#!/usr/bin/python
# -*- coding: UTF-8 -*-

import rospy,os,math
import tf
import time
import json,copy,yaml
import Queue

from std_msgs.msg import Bool
from std_srvs.srv import SetBool
from cln_msgs.msg import RobotSn,UpdateState,VoiceOutput,StringArray,LogUploadStateArray,LogUploadState
from cln_msgs.msg import HardwareState,HardwareCtrl,HardwareStateType,AlarmMsgArray
from cln_msgs.msg import CalibrationStatus,CleanTask,BagStatus,AxSlamState,Maintenance
from cln_msgs.srv import *
from std_srvs.srv import *
from sensor_msgs.msg import LaserScan
# from axbot_py_sdk.srv import * 
# from axbot_py_sdk.msg import AxSlamState 
from threading import Thread
from std_msgs.msg import String,UInt8,Float32MultiArray
# from std_msgs.msg import Empty as EmptyMsg
# from std_srvs.srv import Empty as EmptySrv

from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
from v3_msgs.msg import SyncFile
from v3_msgs.srv import (
    BagsRecord, BagsRecordRequest,
    CloudSyncFileManager, CloudSyncFileManagerRequest
)
# from tool import User_Data_Server

from tool import Log_Parser
log = Log_Parser()

class Robot_Parser:
    def __init__(self):
        self.__node_name = "Robot_Parser"

    def InitParser(self, node_name):
        '''
            初始化
        '''
        ## node name配置
        self.__node_name = node_name
        
        ##
        self.__user_config_dir = rospy.get_param("user_config","/home/aoting/user_config/") 
        ##
        self.__robot_sn_path = os.path.join(self.__user_config_dir,"params/robot_sn.yaml") 

        ##雷达标定模式，0 -默认,1-删除原有数据，重新标定，3-保留原有数据，更新标定
        self.__lidar_calib_mode = 0 

        ##机器人序列号信息
        self.__factory_sn_info = {}


        ##雷达扫描标定板的数据
        self.__lidar_check_info = {}

    
        ## IMU安装标定数据
        self.__imu_calibration_status_info = {}
        self.__imu_calibration_status_info["calibrate_result"] = ""
        self.__calibrate_result_flag = None

        ##所有文件上传文件状态
        self.__logUploadStatelist = LogUploadStateArray()

        ## 当前文件上传的状态
        self.__logUploadState = LogUploadState()

        ## 当前的报警列表
        self.__curr_alarm_events_info = AlarmMsgArray()

        ##音量
        self.__volume = 0 

        ## 电池状态 充放电状态:0放电 1充电
        self.__bat_state = 0
        ## #充电口继电器反馈状态 1 打开 0关闭
        self.__charge_ctrl = 0

        ##云token
        self.cloudplatform_token = ""
        
        ## 地图名称 20230531-dw.liu
        self.__map_name = None

        ## slam_a是否在定位
        self.__slam_a_relocating = False # false默认，当前没有纠定位；true 正在自动纠正定位，需要停止运动
        ## 用户参数配置服务
        # self.__user_data_server = User_Data_Server()

        ## 更新状态
        self.__update_state = UpdateState()
        ## 固件更新状态
        self.__firmware_update_state = UpdateState()

        ## 版本数据
        self.__version_info = UpdateState()

        ## bag状态
        self.__bag_status_msg = BagStatus()

        ##发布topic
        self.__robot_safe_ctrl_pub = rospy.Publisher("/robot_safe_ctrl", Bool, latch=True, queue_size=1)

        self.__rviz_points_pub = rospy.Publisher("/rviz_point", Path, queue_size=15)

        self.__voice_pub = rospy.Publisher("/voice_tts", VoiceOutput, queue_size=15)                        # --update --2023.2.16 --dw.liu

        self.__robot_param_change = rospy.Publisher("/robot_param_change",StringArray,queue_size=1)   #发布 参数改变 --update 2023.2.28 dw.liu
        
        self.__charge_ctrl_pub = rospy.Publisher("/charge_ctrl", HardwareCtrl, queue_size=1)
        
        ### 发布，用于控制地毯模式（地毯电流）
        self.__stall_Mode_Current_pub = rospy.Publisher("/stall_Mode_Current", String, queue_size=1) # 20230531-dw.liu
        ### 清扫任务状态控制
        self.__task_ctrl_pub = rospy.Publisher("/exe_state_ctrl", UInt8, queue_size=1)

        self.cloudPlatform_bindElevator_publisher = rospy.Publisher(
            "cloudPlatform_bindElevator", Bool, queue_size=1, latch=True)           # 机器是否绑定梯空 20230907-dw.liu

        #self.__robot_depth_camera_param_change = rospy.Publisher("/depth_params_update", Empty ,queue_size=1)
        ##订阅topic
        rospy.Subscriber("/robot_sn", RobotSn, self.RobotSn_Callback,queue_size=1)
        
        rospy.Subscriber("/software_update_state", UpdateState, self.UpdateState_Callback,queue_size=1)
        
        rospy.Subscriber("/firmware_update_state", UpdateState, self.FirmwareUpdateState_Callback,queue_size=1)

        rospy.Subscriber("/hardware_state", HardwareState, self.HardwareState_Callback, queue_size=1)

        rospy.Subscriber("/cloudplatform_token",String,callback=self.CloudPlatformTokenCallback)
  
        ### 订阅所有文件上传文件状态话题
        rospy.Subscriber("/bag_upload_list",LogUploadStateArray,self.BagUploadList_Callback,queue_size=1)
        ### 订阅当前上传文件状态话题
        rospy.Subscriber("/bag_upload_current",LogUploadState,self.BagUploadCurrent,queue_size=1)
        ### 雷达扫描标定板的数据
        rospy.Subscriber("/lidar_check",CalibrationStatus,self.LidarCheck_Callback,queue_size=1)
        ### IMU安装标定数据
        rospy.Subscriber("/imu_calibration_status",CalibrationStatus,self.ImuCalibrationStatus_Callback,queue_size=1)
        ### 订阅当前的报警列表
        rospy.Subscriber("/curr_alarm_events",AlarmMsgArray,self.CurrAlarmEvents_Callback,queue_size=1)
        ### 订阅地图名称
        rospy.Subscriber("/map_name", String, callback=self.MapName_Callback,queue_size=1) # 20230531-dw.liu
        ### 订阅正在录制bag的状态
        rospy.Subscriber("/bag_status", BagStatus, callback=self.BagStatus_Callback,queue_size=1) # 20230914-dw.liu
        ### 订阅slam_a定位质量话题
        rospy.Subscriber("/ax_slam_state",AxSlamState,callback=self.AxSlamState_Callback,queue_size=1) # 20231121-dw.liu
        ##服务Proxy
        self.__relocation_client = rospy.ServiceProxy('/robot_relocation',MarkerPointCmd)

        self.__docker_relocation_client = rospy.ServiceProxy('/robot_docker_relocation',MarkerPointCmd)

        self.__lidar_calib_client = rospy.ServiceProxy('/lidar_calibration',SetBool)
        self.__lidar_calib_update_client = rospy.ServiceProxy('/lidar_calibration_update',SetBool)
        self.__generate_robot_sn_client = rospy.ServiceProxy('/generate_robot_sn',GenerateRobotSN)
        self.__volume_adjust_client = rospy.ServiceProxy('/volume_adjust',SetValue)
        self.__software_update_client = rospy.ServiceProxy('/software_update_server',UpdateCheck)
        ### 软件历史版本列表
        self.__software_version_list_client = rospy.ServiceProxy('/software_version_list',SoftwareVersionList)

        self.__firmware_update_client = rospy.ServiceProxy('/firmware_update_server',UpdateCheck)

        self.__firmware_upgrade_client = rospy.ServiceProxy('/firmware_upgrade',FirmwareUpgrade)
        #### 获取本地（软件、硬件）版本数据
        self.__location_version_info_server_client = rospy.ServiceProxy('/location_version_info_server',UpdateCheck)

        self.__reboot_client = rospy.ServiceProxy('/ipc_reboot',Trigger)

        self.__robot_reset_client = rospy.ServiceProxy('/robot_reset',Empty) #数据重置服务

        ### bag查询服务
        self.__bag_query_client = rospy.ServiceProxy('/bag_query',BagQuery)

        ### 清除所有数据库中的日志文件
        self.__log_clean_all_client = rospy.ServiceProxy('/log_clean_all',Trigger)

        ### 雷达出厂校正服务（是否开启）
        self.__lidar2d_calibration_server_client = rospy.ServiceProxy('lidar_calibration_start',Lidar2dCalibration)
    

        ### IMU安装标定
        self.__imu_calibration_server_client = rospy.ServiceProxy('/imu_view_server',SetBool)

        ### IMU SlamA 标定服务
        self.__imu_calibration_server_slam_a_client = rospy.ServiceProxy('/imu_calibration_server',SetBool)

        ### 查询，设置时区
        self.__timezone_server_client = rospy.ServiceProxy('/timezone_server',TimeZone) #20230728-dw.liu
    
        ### 清除手动充电状态，服务 20230731
        self.__chargestate_clean_server_client = rospy.ServiceProxy('/chargestate_clean',Trigger)
        
        ### 结束急停状态 20230802-dw.liu
        self.__stopstate_clean_server_client = rospy.ServiceProxy('/stopstate_clean',Trigger)

        ### 修改机器人序列号服务
        self.__robot_sn_change_server_client = rospy.ServiceProxy('/robot_sn_change',UpdateRobotSN)

        ### 录制bag服务 20230913-dw.liu
        self.__bag_recore_server_client = rospy.ServiceProxy('/bag_recore_server',BagRecord)

        # 矩形区域重定位功能 20231121-dw.liu
        self.__ax_set_pose_client = rospy.ServiceProxy('/ax_set_pose',AxSetPose)


        # 机器维护提醒 -20240423-dw.liu
        self.__maintenance_manager_client = rospy.ServiceProxy('/maintenance_manager',RequestMaintenance)

        #
        self.Send_cloudPlatform_bindElevator()
       

        # ##深度相机标定数据
        # self.__camera_calibration_status_info = {}
        ### 深度相机安装标定数据
        # rospy.Subscriber("/camera_calibration_status",CalibrationStatus,self.CameraCalibrationStatus_Callback,queue_size=1)
        ### 深度相机标定服务
        # self.__camera_calibration_server_client = rospy.ServiceProxy('/camera_calibration_server',CameraCalibration)
        # # 深度相机现场标定数据
        # self.__depth2laser_node_1_calibration_server_client = rospy.ServiceProxy('/depth2laser_node_1_calibration_server',SetBool)
        # self.__depth2laser_node_2_calibration_server_client = rospy.ServiceProxy('/depth2laser_node_2_calibration_server',SetBool)
        # self.__depth2laser_node_3_calibration_server_client = rospy.ServiceProxy('/depth2laser_node_3_calibration_server',SetBool)
        # self.__depth2laser_node_4_calibration_server_client = rospy.ServiceProxy('/depth2laser_node_4_calibration_server',SetBool)
        # self.__depth2laser_node_5_calibration_server_client = rospy.ServiceProxy('/depth2laser_node_5_calibration_server',SetBool)
        # # 联合校准使能话题 20240513-dw.liu
        # self.__calibrate_enable_pub = rospy.Publisher("/calibrate_enable", Bool, queue_size=1)                    
        # ## 一键联合校准结果
        # self.__co_calibration_result = {}   # {camrea_01:{'result':false},...}
        # self.__co_calibration_result_flag = False # 标定结果是否已经获取
        # self.__robot_status = {
        #     'angle':0.0,
        #     'distance':0.0,
        #     'enable':1.0
        # }
        # self.__co_calibration_result_callback_queue = Queue.Queue()
        # 机器人摆放位姿 20240513-dw.liu
        # rospy.Subscriber("/robot_status",Float32MultiArray,callback=self.RobotStatus_Callback,queue_size=1) 

        ### 深度相机标定服务
        self.__depth_camera_calibrate_client = rospy.ServiceProxy('/depth_camera_calibrate_server',CameraCalibration)        
        self.__depth_camera_combined_calibrate_client = rospy.ServiceProxy('/depth_camera_combined_calibrate_server', SetBool)  

        self.__bags_record_client = rospy.ServiceProxy("/bag_record_server", BagsRecord)
        self.__cloudsync_file_client = rospy.ServiceProxy('/cloudsync_file_server',CloudSyncFileManager)

    def Send_cloudPlatform_bindElevator(self):
        '''
        开机启动，发布是否绑定梯控
        '''
        elevlator = rospy.get_param("/elevlator",None)
        if elevlator:
            platform = elevlator.get("platform")
            
            if platform in ["A","B"]:
                self.cloudPlatform_bindElevator_publisher.publish(True) # 绑定
            else:
                self.cloudPlatform_bindElevator_publisher.publish(False) # 不绑定
        else:
            self.cloudPlatform_bindElevator_publisher.publish(False) # 不绑定
        

    
    def GetNodeName(self):
        '''
            获取节点名称
        '''
        return self.__node_name
    #------------------------------

    #--------------------------------------#
    #******#  ROS topic publication #******# 
    #--------------------------------------#
    def PublishRobotSafeTopic(self, is_safe):
        '''
            机器人安全确认话题发布
        '''
        self.__robot_safe_ctrl_pub.publish(is_safe)

    #-----------------------------------#
    #******#  ROS topic Callback #******# 
    #-----------------------------------#
    def RobotSn_Callback(self, info):
        """ 
            机器人序列号信息回调函数
        """
        self.__factory_sn_info["robot_sn"] = info.robot_sn
        self.__factory_sn_info["robot_type"] = info.robot_type 
        self.__factory_sn_info["robot_fun"] = info.robot_fun
        self.__factory_sn_info["robot_class"] = info.robot_class
        self.__factory_sn_info["create_batch"] = info.create_batch 
        self.__factory_sn_info["create_time"] = info.create_time
        self.__factory_sn_info["create_id"] = info.create_id 

    def BagUploadList_Callback(self,msg):
        '''
        所有文件上传文件状态
        '''
        # print(msg)
        self.__logUploadStatelist = msg
        

    def BagUploadCurrent(self,msg):
        '''
        当前上传文件状态
        '''
        self.__logUploadState = msg

    def LidarCheck_Callback(self,msg):
        '''
        雷达扫描标定板的数据
        '''

        # self.__lidar_check_info["sensor_id"] = msg.sensor_id
        # self.__lidar_check_info["front_distance"] = msg.front_distance
        # self.__lidar_check_info["pitch_deg"] = msg.pitch_deg

        self.__lidar_check_info["sensor_id"] = msg.sensor_id
        self.__lidar_check_info["install_x"] = msg.install_x
        self.__lidar_check_info["install_y"] = msg.install_y
        self.__lidar_check_info["install_z"] = msg.install_z
        self.__lidar_check_info["roll_deg"] = msg.roll_deg
        self.__lidar_check_info["pitch_deg"] = msg.pitch_deg
        self.__lidar_check_info["yaw_deg"] = msg.yaw_deg
        self.__lidar_check_info["front_distance"] = msg.front_distance

        
        
        # self.__lidar_check_info["width"] = msg.width        # 对应标定板宽度
        # self.__lidar_check_info["height"] = msg.height      # 对应标定板高度
        # self.__lidar_check_info["is_valid"] = msg.is_valid  # 校正数据是否有效，ture:有效，False:无效
        # self.__lidar_check_info["front_distance"] = msg.front_distance # 雷达正前方距离
        # self.__lidar_check_info["angle"] = msg.angle    # 雷达安装角度

    def ImuCalibrationStatus_Callback(self,msg):
        '''
        IMU安装标定数据
        '''
        self.__imu_calibration_status_info["sensor_id"] = msg.sensor_id

        self.__imu_calibration_status_info["install_x"] = msg.install_x
        self.__imu_calibration_status_info["install_y"] = msg.install_y
        self.__imu_calibration_status_info["install_z"] = msg.install_z
        self.__imu_calibration_status_info["roll_deg"] = msg.roll_deg
        self.__imu_calibration_status_info["pitch_deg"] = msg.pitch_deg
        self.__imu_calibration_status_info["yaw_deg"] = msg.yaw_deg

        self.__imu_calibration_status_info["time_stamp"] = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(msg.time_stamp.secs)) # 时间戳
        # self.__imu_calibration_status_info["calibrate_result"] = msg.calibrate_result # 校准结果
        if msg.calibrate_result:
            if self.__calibrate_result_flag != msg.calibrate_result:
                self.__calibrate_result_flag = msg.calibrate_result
                self.__imu_calibration_status_info["calibrate_result"] = msg.calibrate_result # 校准结果


        # self.__camera_calibration_status_info["front_distance"] = msg.front_distance # 机器人前向距离，正前方0°，用于雷达校准(sensor_id=LIDAR_2D

    def CurrAlarmEvents_Callback(self,msg):
        '''
        报警事件列表 20230515-dw.liu
        '''
        self.__curr_alarm_events_info = msg

    def MapName_Callback(self, msg):
        '''
        地图名称 20230531-dw.liu
        '''
        self.__map_name = msg.data

    
    def CloudPlatformTokenCallback(self,msg):
        '''云序列号回调函数
        '''
        self.cloudplatform_token = msg.data

    def BagStatus_Callback(self,msg):
        '''
        bag正在录制时的状态
        '''
        self.__bag_status_msg = msg

    def AxSlamState_Callback(self,msg):
        '''
        slam_a定位质量话题
        '''
        self.__slam_a_relocating = msg.relocating
    
    
    
    #-----------------------------------#
    #******#  intrinsic function #******# 
    #-----------------------------------#
    def GetTf(self, target_frame, source_frame):
        """ 
            获得机器人坐标变换
        """
        for i in range(10):
            try:
                time.sleep(0.01)
                if not hasattr(self,"tfListener"):
                    self.tfListener = tf.TransformListener()
                (trans,rot)=self.tfListener.lookupTransform(target_frame, source_frame, rospy.Time(0.0))
                (roll,pitch,yaw)=tf.transformations.euler_from_quaternion(rot)
                return True,trans[0],trans[1],yaw
            except Exception as err:
                rospy.logwarn("[{}Failed to get TF<{} -> {}>: {}]".format(self.__node_name,source_frame, target_frame, err))
                pass
        return False,0,0,0
    
    def GetTimezone(self):
        '''
        获取机器人当前的时区
        create-20230728-dw.liu
        '''
        try:
            # 调用服务
            timezone_req = TimeZoneRequest()
            timezone_req.flag = TimeZoneRequest.GET_TIMEZONE # 获取时区
            
            res = self.__timezone_server_client(timezone_req)
            # print(res)

            if not res.success: raise Exception(res.message)
            cur_timezone = res.cur_timezone # 当前时区
            
        except Exception as error:
            cur_timezone = None
        finally:
            return cur_timezone      

    def PubRobotParamChangeTopic(self,data):
        """
        发布机器人参数变更话题
        
        Args:
        - self: 类实例对象
        - data: 包含机器人参数变更信息的字典，格式为 {参数名: 参数值}
        
        Returns:
        - None
        
        """
        update_data = StringArray()
        for key, value in data.items():

            update_data.data.append('/' + key)
            
            # 记录日志
            log_msg = "[Robot_Parser] Successful to set robot parameter {}:{}".format(key, value)
            log.LogInfo(self.GetNodeName(), log_msg)
            
        # 通知其他节点参数变更
        self.__robot_param_change.publish(update_data)
        
    
    def getDeviceControlMap(self):
        ''' 获取外设控制参数 ft.wang 20240605'''
        try:
            device_control_map = rospy.get_param("/device_control_map",{})
            # res_data = {}
            # for key,value in device_control_map.items():
            #     res_data["id_{}".format(key)] = value
            # return res_data 
            return device_control_map
        except Exception as error:
            err_msg = "Error fetching clean width parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  
            
    
    def GetDockerParams(self):
        """
        获取充电桩相关参数
        
        Args:
            无
        
        Returns:
            dict: 包含充电桩相关参数的字典，包括：
                - docker_type (int): 充电桩类型
                - docker_width (float): 充电桩宽度
                - docker_thickness (float): 充电桩厚度
                - docker_distance (float): 对桩滑行点距离
                - docking_force_push_vel (float): 对桩冲击速度
                - docking_force_push_time (float): 对桩冲击时间
                - release_docker_time (float): 退桩时间
                - docking_direction (str): 对桩充电口方向
        
        """
        try:
            charging_station_params  = rospy.get_param("/docker",{})
            
            docker_params  = {
                'docker_type': charging_station_params.get('docker_type',0),
                'docker_width': charging_station_params.get('docker_width',0.43),
                'docker_thickness': charging_station_params.get('docker_thickness',0.17),
                'docker_distance': charging_station_params.get('docker_distance',1.5),
                'docking_force_push_vel': charging_station_params.get('docking_force_push_vel',0.08),
                'docking_force_push_time': charging_station_params.get('docking_force_push_time',1.5),
                'release_docker_time': charging_station_params.get('release_docker_time',1.5),
                'docking_direction': charging_station_params.get('docking_direction','backward'),
                'docker_method': charging_station_params.get('docker_method',0)
            }
            ret_docker_params = {'docker':docker_params} 
            return ret_docker_params 
        except Exception as error:
            err_msg = "Error fetching charging station parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  
    
    def GetCleanWidthParams(self):
        """
        获取清扫宽度参数
        
        Args:
            无
        
        Returns:
            dict: 包含清扫宽度的字典，若获取失败则返回空字典
        
        """
        try:
            clean_width_params = rospy.get_param("/clean_width",{})
            
            clean_width = {
                # 'sweep_width' : clean_width_params.get('sweep_width',0.25),
                'path_width' : clean_width_params.get('path_width',0.2),
                'path_width_min' : clean_width_params.get('path_width_min',0.2),
                'path_width_max' : clean_width_params.get('path_width_max',0.55)
            }
            ret_clean_width_params = {'clean_width':clean_width} 
            return ret_clean_width_params 
        except Exception as error:
            err_msg = "Error fetching clean width parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  
        
    def GetNightModeParams(self):
        """
        获取夜间模式参数
        
        Args:
            无
        
        Returns:
            dict: 包含夜间模式参数的字典，若获取参数失败则返回空字典。
        
        Raises:
            无
        
        """

        try:
            night_mode_params = rospy.get_param("/night_mode",{})
            
            night_mode = {
                'enable' : night_mode_params.get('enable',1),
                'start_time' : night_mode_params.get('start_time',"18:00"),
                'end_time' : night_mode_params.get('end_time',"06:00")
            }
          
            ret_night_mode_params = {'night_mode':night_mode} 
            return ret_night_mode_params 
        except Exception as error:
            err_msg = "Error fetching night mode parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  

    def GetWarnStateParams(self):
        """
        获取报警状态参数
        
        Args:
            无
        Returns:
            dict: 包含报警状态参数的字典，若获取参数失败则返回空字典。
        Raises:
            无
        """

        try:
            warn_state_params = rospy.get_param("/warn_state",{})
            warn_state = {
                'alltask_finished' : warn_state_params.get('alltask_finished',1),
                'alarm_to_user' : warn_state_params.get('alarm_to_user',1),
            }
          
            ret_warn_state_params = {'warn_state':warn_state} 
            return ret_warn_state_params 
        except Exception as error:
            err_msg = "Error fetching warn state parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  
        
    def GetElevlatorParams(self):
        """
        获取电梯参数
        Args:
            无
        Returns:
            dict: 包含电梯参数的字典，若获取参数失败则返回空字典。
        Raises:
            无
        """

        try:
            elev_params = rospy.get_param("/elevlator",{})
            elev = {
                'yuji_deviceSecret': elev_params.get('yuji_deviceSecret','deviceSecret'),
                'yuji_lora_id': elev_params.get('yuji_lora_id','lora_id'),
                'yuji_place_id': elev_params.get('yuji_place_id','place_id'),
                'yuji_lift_id': elev_params.get('yuji_lift_id','lift_id'),
                'platform': elev_params.get('platform','0')
            }
          
            ret_elev_params = {'elevlator':elev} 
            return ret_elev_params 
        except Exception as error:
            err_msg = "Error fetching elevator parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  
    
    def GetChargeParams(self):
        """
        获取充电参数
        Args:
            无
        Returns:
            dict: 包含充电参数的字典，若获取参数失败则返回空字典。
        Raises:
            无
        """
    
        try:
            charge_params = rospy.get_param("/charge",{})
            charge = {
                'charge_time': charge_params.get('charge_time',200),
                'max_battery_value': charge_params.get('max_battery_value',95),
                'gohome_power': charge_params.get('gohome_power',20),
                'shutdown_power': charge_params.get('shutdown_power',10)
            }
          
            ret_charge_params = {'charge':charge} 
            return ret_charge_params 
        except Exception as error:
            err_msg = "Error fetching charging parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  
        
    def GetWorkingVoiceParams(self):
        """
        获取工作语音参数
        Args:
            无
        Returns:
            dict: 包含工作语音参数的字典，若获取参数失败则返回空字典。
        Raises:
            无
        """
    
        try:
            voice_params = rospy.get_param("/working_voice",{})
            voice = {
                'frequency': voice_params.get('frequency',60),
                'enable': voice_params.get('enable',False),
                'data': voice_params.get('data',"努力工作中，请不要打扰"),
                'data_en': voice_params.get('data',"Working , please do not disturb."),
            }
          
            ret_voice_params = {'working_voice':voice} 
            return ret_voice_params 
        except Exception as error:
            err_msg = "Error fetching working voice parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {} 
 
    def GetNavParams(self):
        """
        获取导航参数
        Args:
            无
        Returns:
            dict: 包含导航参数的字典，若获取参数失败则返回空字典。
        Raises:
            无
        """
    
        try:
            nav_params = rospy.get_param("/nav",{})
            nav = {
                'dynamicObstaclewaitingTime': nav_params.get('dynamicObstaclewaitingTime',5)
            }
          
            ret_nav_params = {'nav':nav} 
            return ret_nav_params 
        except Exception as error:
            err_msg = "Error fetching nav parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}
        
    def GetCleanTaskParams(self):
        """
        获取工作相关参数
        Args:
            无
        Returns:
            dict: 包含导航参数的字典，若获取参数失败则返回空字典。
        Raises:
            无
        """
    
        try:
            clean_task_params = rospy.get_param("/cleantask",{})
            clean_task = {
                'all_finished': clean_task_params.get('all_finished',1),
                'work_time_enable': clean_task_params.get('work_time_enable',False),
                'work_time': clean_task_params.get('work_time',"22:00-07:00")
            }
          
            ret_clean_task_params = {'cleantask':clean_task} 
            return ret_clean_task_params 
        except Exception as error:
            err_msg = "Error fetching clean task parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}
        
        
        


    def RobotDisinfectParamSet(self,data):
        '''
        消杀参数设置
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data: raise Exception("Request data error!")


            spray_interval = data.get("spray_interval", 0)# 喷雾间隔 单位分钟 0时持续喷雾 默认0
            low_warn = data.get("low_warn", 10)# 低于该值时 报水位低报警 0-100 默认10
            high_warn = data.get("high_warn", 90)# 高于该值时 报水位高报警 0-100 默认90
            
            spray_list = rospy.get_param("/spray_list",['disinfect_l','disinfect_r'])

            spray_params_dict = {}   
            
            if spray_interval == 0:
                spray_mode = "Continuous"  # 持续喷雾  = 0
            else:
                spray_mode = "Intermittent"   # 间歇模式  ！= 0
            
            spray_params_dict['spray_mode'] = spray_mode
            
            for key in spray_list:
                
                spray_param = {
                        key: {
                        'spary_open_time': spray_interval if spray_mode == "Intermittent" else 0,  # 喷雾间隔时间
                        'spary_sleep_time': spray_interval if spray_mode == "Intermittent" else 0, # 睡眠时间
                        'liquid_low_level' : low_warn,
                        'liquid_hight_level': high_warn
                    }
                    
                }
                spray_params_dict.update(spray_param)

            # 从字典类型的数据中设置ROS参数。
            self.SetParams(spray_params_dict)
            
            # 通知其他节点参数变更
            self.PubRobotParamChangeTopic(spray_params_dict)
                    
 
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][RobotDisinfectParamSet]Failed to set disinfect parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
     
        return ret_dat     


    def SetRobotDepthCameraParams(self,data):
        """
        设置机器人深度相机参数
        
        Args:
            data (dict): 包含深度相机参数的字典
            {u'camera_01': {u'camera': {u'serial_number': u'CH2A133002K'}}, 
            u'depth2laser_node_1':{u'limit': {u'top': 0.344, u'bottom': 0.0}, u'install_pose': {u'yaw': 0.13, u'z': 0.0, u'roll': -47.0, u'pitch': 0.0}}}
        
        Returns:
            dict: 包含操作结果的字典，成功时返回{"msg": "success", "code": 0}，失败时返回包含错误信息的字典
        
        """
        response_data = {"msg": "success", "code": 0}
        
        # 判断请求数据是否为空
        if not data:
            response_data["code"] = -1
            response_data["msg"] = "Request data error!"
            error_msg = "[Robot_Parser]Failed to set robot depth camera parameters: Request data error!"
            log.LogError(self.GetNodeName(), error_msg)
            return response_data
        try:
            # 设置 user_params.yaml 文件中的 depth2laser_node_* 参数
            self.SetParams(data)
            
            changed_param_paths = StringArray()
            
            for key, value in data.items():
                if key.startswith("depth2laser_node_"):
                    camera_id = key.split("_")[-1]
                    install_pose = rospy.get_param("depth_camera_config",{}).get("depth_camera_{}".format(camera_id), {}).get('install_pose', [])
                    if len(install_pose) == 6:
                        install_pose[0] = value['install_pose']['x']
                        install_pose[1] = value['install_pose']['y']
                        install_pose[2] = value['install_pose']['z']
                        install_pose[3] = value['install_pose']['roll']
                        install_pose[4] = value['install_pose']['pitch']
                        install_pose[5] = value['install_pose']['yaw']
                    else:
                        install_pose = [value['install_pose']['x'], value['install_pose']['y'], value['install_pose']['z'], value['install_pose']['roll'], value['install_pose']['pitch'], value['install_pose']['yaw']]
                    
                    rospy.set_param("depth_camera_config/depth_camera_0{}/install_pose".format(camera_id), install_pose)

                    changed_param_paths.data.append('/' + key)
                    # 记录日志
                    log_msg = "[Robot_Parser] Successful to set robot parameter {}:{}".format(key, value)
                    log.LogInfo(self.GetNodeName(), log_msg)
            
            if changed_param_paths.data:  # 检查是否有任何参数被更改
                changed_param_paths.data.append('/depth_camera_config')
                    
            # 通知其他节点参数变更
            self.__robot_param_change.publish(changed_param_paths)         
        except Exception as error:
            response_data["code"] = -1
            response_data["msg"] = str(error)

            err_msg ="[Robot_Parser]Failed to set robot depth camera parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

      
        return response_data               
 

    def GetRobotPose_TF(self):
        '''
            通过TF方式获得机器人坐标
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            state,x,y,yaw = self.GetTf('/map','/base_link')
            if state:
                ret_dat["pose_x"] = x
                ret_dat["pose_y"] = y
                ret_dat["pose_yaw"] = yaw
            else:
                raise Exception("Get tf error!")
            log_msg = "[Robot_Parser]Successful to get robot pose."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to get robot pose: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat          

    def VerifyRobotSafe(self, data):
        '''
            机器人位置安全确认
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data is empty!")
  
            robot_safe = data["robot_safe"]
            self.PublishRobotSafeTopic(True if robot_safe == 1 else False)

            log_msg = "[Robot_Parser]Successful to verify robot safe."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to verify robot safe: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    def RobotLock(self, data):
        '''
            锁机
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data is empty!")
  
            robot_lock = data["lock"]

            # 定位确认 robot_lock = 1，表示锁机，取消安全确认
            if robot_lock == 1:
                self.PublishRobotSafeTopic(False)

            # 设置参数
            rospy.set_param("/robot_lock_state", robot_lock)
            # 通知其它节点参数发生改变
            update_data = StringArray()
            temp = "/robot_lock_state"
            update_data.data.append(temp)
            self.__robot_param_change.publish(update_data)


            log_msg = "[Robot_Parser]Successful to lock robot ."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:

            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to lock robot err: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
 
        return ret_dat
        
        

    def HandChargeOff(self):
        '''
            结束手动充电
            create-20230726-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            
            # 调用服务或者发送话题（）

            res = self.__chargestate_clean_server_client.call(TriggerRequest())
            if not res.success: raise Exception("service /chargestate_clean: {}".format(res.message))


            log_msg = "[Robot_Parser][HandChargeOff] Successful to hand charge off ."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][HandChargeOff] Failed to hand charge off err: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    
    def EmergencyOff(self):
        '''
            结束急停状态
            create-20230726-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            
            # 调用服务或者发送话题（）

            res = self.__stopstate_clean_server_client.call(TriggerRequest())
            if not res.success: raise Exception("service /chargestate_clean: {}".format(res.message))

           # self.__task_ctrl_pub.publish(CleanTask.EXECUTING)   # 清扫任务状态控制话题，清扫任务恢复 20230901-dw.liu

            log_msg = "[Robot_Parser][EmergencyOff] Successful to emergency off ."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][EmergencyOff] Failed to emergency off err: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
    
    def RobotSetTimezone(self,data):
        '''
            设置机器人时区
            create-20230728-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data is empty!")
 
            timezone = data["id"]
            # 调用服务或者发送话题（）
            timezone_req = TimeZoneRequest()
            timezone_req.flag = TimeZoneRequest.SET_TIMEZONE # 设置时区
            timezone_req.timezone = timezone
            res = self.__timezone_server_client(timezone_req)


            if not res.success: raise Exception(res.message)

            log_msg = "[Robot_Parser][RobotSetTimezone] Successful to set timezone ."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][RobotSetTimezone] Failed to set timezone err: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
        return ret_dat
        


    def RobotFunction(self):
        '''
            机器人支持的新功能
            create-20230927-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            function_enable = rospy.get_param("/function_enable",{})

            function_enable_dict = {
                'map_update': self.ConvertBool(function_enable.get('map_update_enable',False)),# 局部更新功能使能
                'elevator_enable': self.ConvertBool(function_enable.get('elevator_enable',False)),# 梯控功能使能
                'access_control':self.ConvertBool(function_enable.get('gate_enable',False)),# 门禁闸机功能使能
                'disinfect_enable':self.ConvertBool(function_enable.get('disinfect_enable',False)),# 消毒喷雾功能使能
                'ad_enable':self.ConvertBool(function_enable.get('ad_enable',False)),# 广告功能使能 
                'camera_docker_enable':self.ConvertBool(function_enable.get('camera_docker_enable',False)),# 相机回桩功能使能 
                'clamshell_enable':self.ConvertBool(function_enable.get('clamshell_enable',False)),# 翻盖推杆功能使能 
                'function_zone_enable':self.ConvertBool(function_enable.get('function_zone_enable',False)),# 功能区功能
                'reflective_panel_enable':self.ConvertBool(function_enable.get('reflective_panel_enable',False)),# 反光板功能使能
            }
            
        
            ret_dat.update(function_enable_dict)
          
            log_msg = "[Robot_Parser][RobotFunction] Successful to get robot function ."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][RobotFunction] Failed to get robot function err: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
        return ret_dat

    def GetDustboxTime(self):
        '''
            尘盒时间查询
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # read_dictionary = self.__user_data_server.ReadDustboxTime()
            # ret_dat.update(read_dictionary)
            log_msg = "[Robot_Parser]Successful to get dustbox time."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to get dustbox time: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def SetDustboxTime(self):
        '''
            尘盒时间设置
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # self.__user_data_server.WriteDustboxTime()
            log_msg = "[Robot_Parser]Successful to set dustbox time."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to set dustbox time: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat
    
    def MaintenanceGet(self):
        '''
            维护保养剩余时间查询
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # 调用服务获取维护保养时间
            req = RequestMaintenanceRequest()
            req.option = RequestMaintenanceRequest.QUERY
            res = self.__maintenance_manager_client(req)
            
            if res.success == False: raise Exception(res.message)
            
            maintenance_list = res.maintenance_list
   
            ret_data = []
            for maintenance_one in maintenance_list:
                maintenance_dict ={}
                maintenance_dict["maintenance_id"]  = maintenance_one.maintenance_id 
                maintenance_dict["value_threshold"] = math.ceil(maintenance_one.value_threshold /60.0/60.0)
                maintenance_dict["value"] = math.ceil(maintenance_one.value /60.0/60.0)
                ret_data.append(maintenance_dict)
            
            ret_dat["data"] = ret_data
            
            
            log_msg = "[Robot_Parser]Successful to get remaining time for maintenance"
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:

            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to get remaining time for maintenance: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
        finally:
            return ret_dat
        

    def MaintenanceSet(self,data):
        '''
            维护保养已保养
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            req = RequestMaintenanceRequest()
            req.option = RequestMaintenanceRequest.RESET
            for maintenance_one in data:
                maintenance = Maintenance()
                maintenance.maintenance_id = maintenance_one
                req.maintenance_list.append(maintenance)
            # 调用服务设置重置维护保养时间
            print("调用服务设置重置维护保养时间: ",req)
            res = self.__maintenance_manager_client(req)
            if res.success == False: raise Exception(res.message)
        
            log_msg = "[Robot_Parser]Successful to set remaining time for maintenance."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:

            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to set remaining time for maintenance: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
        finally:
            return ret_dat
        
        

    def StartDockerRelocation(self):
        '''
            开始充电桩重定位
            create:2023.2.6  --dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # 检测充电口是否已经打开
            if self.__charge_ctrl == 1:
                # 已经打开 检测是否处于充电状态
                if self.__bat_state == 1:
                    relocation_req = MarkerPointCmdRequest()
                    relocation_req.map_name = self.__map_name
                    res = self.__relocation_client.call(relocation_req)

                    if not res.success: 
                        error_msg = "robot_relocation  service_error"   #定位服务内部错误

                        err_msg = "[Robot_Parser]Failed to relocate robot: {}".format(res.message) # 记录错误
                        log.LogError(self.GetNodeName(),err_msg)
                        raise Exception(error_msg)

                    if self.__charge_ctrl == 0:
                        # 定位成功后检测到，充电状态接口关闭，   执行一下关闭操作
                        state = HardwareStateType()
                        state.state = state.OFF
                        cmd = HardwareCtrl()
                        cmd.state.append(state)     
                        self.__charge_ctrl_pub.publish(cmd)
                        rospy.set_param('/robot/electrode_state', 0)


                    log_msg = "[Robot_Parser]Successful to relocate robot."
                    log.LogInfo(self.GetNodeName(),log_msg)
                else:                                           
                    # 位置没对好，没充上电  或者 充电桩没有电，没充上电
                    raise Exception("Please recalibrate the position OR There is no power at the charging pile")
            else:
                # 检测充电口，没有打开，  打开   
                state = HardwareStateType()
                state.state = state.ON 
                cmd = HardwareCtrl()
                cmd.state.append(state)     
                self.__charge_ctrl_pub.publish(cmd)
                rospy.set_param('/robot/electrode_state', 1)

                time.sleep(3)                           # 缓冲3秒，在检测机器人的充电状态
                if self.__bat_state == 1:
                    # 这个时候要关闭充电开关，以免影响任务执行，  关闭   # 放在线程里执行
                    docker_relocation_thread = Thread(target=self.charge_ctrl_OFF)
                    docker_relocation_thread.setDaemon(True)
                    docker_relocation_thread.start()

                    # 重定位
                    relocation_req = MarkerPointCmdRequest()
                    relocation_req.map_name = self.__map_name
                    res = self.__relocation_client.call(relocation_req)

                    if not res.success: 
                        error_msg = "robot_relocation  service_error"   #定位服务内部错误

                        err_msg = "[Robot_Parser]Failed to relocate robot: {}".format(res.message) # 记录错误
                        log.LogError(self.GetNodeName(),err_msg)
                        raise Exception(error_msg)

                    log_msg = "[Robot_Parser]Successful to relocate robot."
                    log.LogInfo(self.GetNodeName(),log_msg)
                else:
                    # 这个时候要关闭充电开关，以免影响任务执行，  关闭   # 对桩失败也要关闭充电口 20230828-dw.liu
                    state = HardwareStateType()
                    state.state = state.OFF
                    cmd = HardwareCtrl()
                    cmd.state.append(state)     
                    self.__charge_ctrl_pub.publish(cmd)
                    rospy.set_param('/robot/electrode_state', 0)

                    raise Exception("Please recalibrate the position OR There is no power at the charging pile")

        except Exception as error:
            if "service_error" in str(error):
                #定位服务内部错误
                ret_dat["code"]=0x01070101
                ret_dat["msg"]= "机器人重新定位服务异常"
            elif "unavailable" in str(error):
                # 这个服务没有工作
                ret_dat["code"]=0x02070102
                ret_dat["msg"]= "机器人重新定位服务失效"
            elif "recalibrate" in str(error):
                # 对桩不成功
                ret_dat["code"]=0x02000104
                ret_dat["msg"]= "对桩不成功"
            else:
                # flask
                ret_dat["code"]= 0x02000003
                ret_dat["msg"]= "请求失败"


            err_msg = "[Robot_Parser]Failed to relocate robot: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

        finally:
            return ret_dat
        
    def charge_ctrl_OFF(self):
        '''
        线程函数，关闭充电接口
        create-20230828-dw.liu
        '''
        time.sleep(5)                                   # 延时5s后关闭充电接口
        state = HardwareStateType()
        state.state = state.OFF
        cmd = HardwareCtrl()
        cmd.state.append(state)     
        self.__charge_ctrl_pub.publish(cmd)
        rospy.set_param('/robot/electrode_state', 0)

    
    
    # def StartRelocation(self, data):
    #     '''
    #         开始重定位
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     try:
    #         if not data : raise Exception("Request data error!")
    #         if not data["map_name"] : raise Exception("Map name is empty!")
    #         if not data["tag_name"] : raise Exception("Tag name is empty!")

    #         relocation_req = MarkerPointCmdRequest()
    #         relocation_req.map_name = data["map_name"].encode('utf-8')
    #         relocation_req.tag_name = data["tag_name"].encode('utf-8')
    #         res = self.__relocation_client.call(relocation_req)
    #         if not res.success: raise Exception(res.message)
            
    #         log_msg = "[Robot_Parser]Successful to relocate robot."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser]Failed to relocate robot: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #         # rospy.logerr(err_msg)
    #     finally:
    #         return ret_dat
        
    # def RelocationPoint(self, data):
    #     '''
    #         地图上选一个点重新定位
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     try:
    #         if not data : raise Exception("Request data error!")
    #         print(data)
    #         relocation_req = AxSetPoseRequest()
    #         relocation_req.adjust_position = True #adjust_position 为 True 时在设置的位姿周围 2m 内来自动纠正定位
    #         relocation_req.x = data["point"]['x']
    #         relocation_req.y =  data["point"]['y']
    #         relocation_req.ori = 0
    #         print(relocation_req)
    #         res = self.__ax_set_pose_client.call(relocation_req)
    #         if not res.success: raise Exception(res.message)
    #         time_start = time.time()
    #         time.sleep(1)
    #         while True:
    #             if not self.Timeout_Exception(time_start,15):
    #                 raise Exception('relocate time out')  
    #             if self.__slam_a_relocating == False: #定位结束了
    #                 break

    #         log_msg = "[Robot_Parser]Successful to relocate robot by slam_a."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser]Failed to relocate robot by slam_a: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
    #         # rospy.logerr(err_msg)
    #     finally:
    #         return ret_dat
    
    def Timeout_Exception(self,time_start,time_interval):
        '''
        计时结束后抛出错误
        '''
        time_now = time.time()
        if (time_now - time_start) > time_interval:
            return False
        else:
            return True



    def GetVolume(self):
        '''
            获取音量
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            ret_dat["data"] = rospy.get_param('/volume', self.__volume)
            log_msg = "[Robot_Parser]Successful to get volume."
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat
        except Exception as error:
            rospy.logerr("[Robot_Parser]Failed to get volume: {}".format(error))
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)
            return ret_dat

    def SetVolume(self, data):
        '''
            设置音量
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data is empty!")
            # if not data["volume"] : raise Exception("Volume is empty!")
            
            volume = data["volume"]
            req = SetValueRequest()
            req.int8_value = volume
            res = self.__volume_adjust_client.call(req)
            if not res.success: raise Exception("Service</volume_adjust> error: {}".format(res.message))
            self.__volume = res.int8_value

            voice = VoiceOutput()                           # --update --2023.2.16 --dw.liu
            #voice.voice_data = "音量调节完成啦"
            voice.default_voice_key = "volume_adjust"
            self.__voice_pub.publish(voice)
            log_msg = "[Robot_Parser]Successful to set volume."
            log.LogInfo(self.GetNodeName(),log_msg)
            
            return ret_dat
        except Exception as error:
            rospy.logerr("[Robot_Parser]Failed to set volume: {}".format(error))
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)
            return ret_dat

    def QueryLogs(self, data):
        '''
        请求日志信息
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")
            
            start_time = data["start_time"]
            end_time = data["end_time"]  
            log_level = data["log_level"]

            success,msg,logs = log.QueryLogs(start_time,end_time,log_level)
            if not success:  raise Exception(msg)

            ret_dat["log"] = logs
            log_msg = "[Robot_Parser]Successful to query logs."
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat
        except Exception as error:
            ret_dat["code"]=-1 
            #ret_dat["ret"]='fail'
            ret_dat["msg"]=str(error)
            rospy.logerr("[Robot_Parser][Log_Parser]Failed to query logs: {}".format(error))
            return ret_dat
        
    def UploadLogs(self, data):
        '''
        日志上传到云平台
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")
            
            start_time = data["start_time"]
            end_time = data["end_time"]  
            # log_level = data["log_level"]
            if not start_time: raise Exception("start time is empty!")   
            if not end_time: raise Exception("end time is empty!") 
            
            upload_logs_req = BagQueryRequest()
            # bool 查询后是否开始准备上传bag文件
            upload_logs_req.is_upload = True
            upload_logs_req.start_time = log.ConvertStringtoTime(start_time)
            upload_logs_req.end_time = log.ConvertStringtoTime(end_time)

            res =self.__bag_query_client(upload_logs_req)
            if not res.success: raise Exception("service /bag_query error: {}".format(res.message))

            bag_list = []
            
            # 遍历bag列表
            for bag_list_one in res.bag_list:

                # print(bag_list_one)
                # print(".........")
                # 转换
                bag_info_one = {}
                bag_info_one["file_name"] = bag_list_one.file_name
                bag_info_one["file_path"] = bag_list_one.file_path
                bag_info_one["create_time"] = bag_list_one.create_time
                bag_info_one["end_time"] = bag_list_one.end_time
                bag_info_one["file_size"] = bag_list_one.file_size
                bag_info_one["is_upload"] = bag_list_one.is_upload
                bag_info_one["is_delete"] = bag_list_one.is_delete
                bag_info_one["upload_time"] = bag_list_one.upload_time
                bag_info_one["progress"] = bag_list_one.progress

                bag_list.append(bag_info_one)
            
            #print(bag_list)
            ret_dat["bag_list"] = bag_list
            log_msg = "[Robot_Parser][UploadLogs]Successful to upload logs."
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat
        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)
            # rospy.logerr("[Robot_Parser][UploadLogs]Failed to upload logs: {}".format(error))

            log_msg = "[Robot_Parser][UploadLogs]Failed to upload logs: {}".format(error)
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat
        
    def GetProgressLogs(self):
        '''
        查询日志上传进度
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # print(self.__logUploadState)
            # 注意进度目前只有/bag_upload_current接口发布，但还有些问题，只有上传完成时才返回100%，还需要完善
            # 当前上传文件状态话题(/bag_upload_current)
            # 所以现在返回的列表里只有一个文件的数据
            logUploadState = {}
            logUploadState["file_name"] = self.__logUploadState.file_name
            logUploadState["file_path"] = self.__logUploadState.file_path
            logUploadState["create_time"] = self.__logUploadState.create_time
            logUploadState["end_time"] = self.__logUploadState.end_time
            logUploadState["file_size"] = self.__logUploadState.file_size
            logUploadState["is_upload"] = self.__logUploadState.is_upload
            logUploadState["is_delete"] = self.__logUploadState.is_delete
            logUploadState["upload_time"] = self.__logUploadState.upload_time
            logUploadState["progress"] = self.__logUploadState.progress

            data = []
            data.append(logUploadState)

            ret_dat["data"] = data
            
            log_msg = "[Robot_Parser][GetProgressLogs]Successful to get progrees!"
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat
        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)
            # rospy.logerr("[Robot_Parser][Log_Parser]Failed to upload logs: {}".format(error))
            
            log_msg = "[Robot_Parser][GetProgressLogs]Failed to get progrees!"
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat

        
    def CleanLogs(self):
        '''
        清空日志
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            res = self.__log_clean_all_client.call(TriggerRequest())
            if not res.success: raise Exception("service /log_clean_all: {}".format(res.message))
            
            log_msg = "[Robot_Parser][CleanLogs]Successful to clean logs."
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat
        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)
            # rospy.logerr("[Robot_Parser][Log_Parser]Failed to upload logs: {}".format(error))

            log_msg = "[Robot_Parser][CleanLogs]Failed to clean logs,error: {}".format(error)
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat


    def GetLogLevelInfo(self):
        '''
        请求日志等级信息列表
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            success,msg,log_levels = log.GetLogLevelInfo()
            if not success:  raise Exception(msg)

            ret_dat["log_level"] = log_levels
            log_msg = "[Robot_Parser]Successful to get log level info."
            log.LogInfo(self.GetNodeName(),log_msg)
            return ret_dat
        except Exception as error:
            ret_dat["code"]=-1 
            #ret_dat["ret"]='fail'
            ret_dat["msg"]=str(error)
            rospy.logerr("[MapParser]Failed to get log level info: {}".format(error))
            return ret_dat 

    def UpdateState_Callback(self,msg):
        '''获取机器人版本信息'''
        # rospy.loginfo("---UpdateState_Callback --")
        self.__update_state = msg

    def FirmwareUpdateState_Callback(self,msg):
        '''获取机器人固件版本信息'''
        self.__firmware_update_state = msg
        
    """ 
        硬件状态回调函数
    """
    def HardwareState_Callback(self,msg_data):
        self.__bat_state = msg_data.bat_state
        self.__charge_ctrl = msg_data.charge_ctrl
        # if 1 == msg_data.e_stop:  ##急停
        #     self.PublishRobotSafeTopic(False)
        #     log_msg = "[Robot_Parser]Publish robot_safe = False (e_stop)."
        #     log.LogInfo(self.GetNodeName(),log_msg)

    def GetVersionInfo(self):
        '''
        获取版本信息
     
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            version_data = {
                "type":"",
                "sn": "",
                "versionNew": "",
                "versionNew": "",
                "versionSize": str(0.0),
                "uploadLog": "",
                "updateLog_Hardware": "",
                "versionNew_Hardware": "",
                "versionNow_Hardware": "",
                "token": "",
            }
            if self.__factory_sn_info.has_key("robot_type"):
                version_data["type"] = self.__factory_sn_info["robot_type"]
            if self.__factory_sn_info.has_key("robot_sn"):
                version_data["sn"] = self.__factory_sn_info["robot_sn"]
            version_data["token"] = self.cloudplatform_token
            if not version_data["sn"]:                                                                                          # 保证信息读取-20231011-dw.liu
                robot_sn = self.GetRobotSn()
                if "robot_type" in robot_sn:
                     version_data["type"] = robot_sn["robot_type"]
                if "robot_sn" in robot_sn:
                    version_data["sn"] = robot_sn["robot_sn"]

            # 新添服务获取本地版本信息
            req = UpdateCheckRequest()
            ret_location_version = self.__location_version_info_server_client.call(req)
            if ret_location_version.success:
                version_data["versionNow"] = ret_location_version.update_info.local_version
                version_data["versionNow_Hardware"] = ret_location_version.update_info.local_version_fireware
            # 改用线程的方式获取云版本信息，加快响应速度
            get_cloud_thread = Thread(target=self.get_cloud_thread)
            get_cloud_thread.setDaemon(True)
            get_cloud_thread.start()



            # req = UpdateCheckRequest()
            # req.flag = 0
            # res_firmware_data = self.__firmware_update_client.call(req)
            # print(res_firmware_data)

            # if res_firmware_data.success:
            #     version_data["versionNow"] = res_firmware_data.update_info.local_version
            #     version_data["versionNew"] = res_firmware_data.update_info.cloud_version
            #     version_data["uploadLog"] = res_firmware_data.update_info.update_log
            #     version_data["versionSize"] = str(int(res_firmware_data.update_info.update_size))
            #     version_data["versionNow_Hardware"] = res_firmware_data.update_info.local_version_fireware
            #     version_data["versionNew_Hardware"] = res_firmware_data.update_info.cloud_version_fireware
            #     version_data["updateLog_Hardware"] = res_firmware_data.update_info.update_log_fireware

            
            # version_data["versionNow"] = self.__version_info.local_version
            version_data["versionNew"] = self.__version_info.cloud_version
            version_data["uploadLog"] = self.__version_info.update_log
            version_data["versionSize"] = str(int(self.__version_info.update_size))
            # version_data["versionNow_Hardware"] = self.__version_info.local_version_fireware
            version_data["versionNew_Hardware"] = self.__version_info.cloud_version_fireware
            version_data["updateLog_Hardware"] = self.__version_info.update_log_fireware


            
            ret_dat["data"] = version_data
            
            
            # log_msg = "[Robot_Parser][GetVersionInfo]Successful to get version info：{}.".format(json.dumps(version_data))
            # log_msg = "[Robot_Parser][GetVersionInfo]Successful to get version info."
            # print(log_msg)
            # log.LogInfo(self.GetNodeName(),log_msg)
       
        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)

            # rospy.logerr("[RobotParser]Failed to get version info: {}".format(error))
            err_msg = "[RobotParser][GetVersionInfo]Failed to get version info: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
        finally:
            return ret_dat
        
    def GetRobotSn(self):
        yaml_data={}
        if os.path.exists(self.__robot_sn_path):
            with open(self.__robot_sn_path,'r') as f: 
                yaml_data = yaml.load(f,Loader=yaml.Loader)
        return yaml_data
        
        

    def get_cloud_thread(self):
        '''
        从云端获取相应版本信息
        '''
        # self.__version_info

        req = UpdateCheckRequest()
        req.flag = 0
        res_firmware_data = self.__firmware_update_client.call(req)
        
        if res_firmware_data.success:
            self.__version_info.local_version= res_firmware_data.update_info.local_version
            self.__version_info.cloud_version = res_firmware_data.update_info.cloud_version
            self.__version_info.update_log = res_firmware_data.update_info.update_log
            self.__version_info.update_size = res_firmware_data.update_info.update_size
            self.__version_info.local_version_fireware = res_firmware_data.update_info.local_version_fireware
            self.__version_info.cloud_version_fireware= res_firmware_data.update_info.cloud_version_fireware
            self.__version_info.update_log_fireware = res_firmware_data.update_info.update_log_fireware
       
        
    
        
    def GetSoftwareUpdateProgress(self):
        '''
        获取版本信息
        软件的下载进度
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            version_data = {
                "progress": str(0.0),
            }
            rospy.loginfo("-----GetSoftwareUpdateProgress-------")
            # print(self.__update_state)
            # if self.__update_state:
            
            version_data["progress"] = str(self.__update_state.update_rate)
            ret_dat["data"] = version_data
            log_msg = "[Robot_Parser][GetSoftwareUpdateProgress]Successful to get version info,progress:{}".format(self.__update_state.update_rate)
            print(log_msg)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)
            rospy.logerr("[RobotParser][GetSoftwareUpdateProgress]Failed to get version info: {}".format(error))
        finally:
            return ret_dat 

    def UpdateVersion(self,data):
        '''
        下载软件版本
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # if not data : raise Exception("Request data error!")

            req = UpdateCheckRequest()
            req.flag = 1
            if data and data.has_key("id"):
                req.id = data["id"].encode()
            print(req)
            
            res = self.__software_update_client.call(req)
            print(res)
            
            
            log_msg = "[Robot_Parser][UpdateVersion]Successful to update software version."
            log.LogInfo(self.GetNodeName(),log_msg)

        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)

            rospy.logerr("[Robot_Parser][UpdateVersion]Failed to update software version: {}".format(error))
        finally:
            return ret_dat
    
    def SoftwareVersionList(self):
        '''
        软件历史版本
        20230524-dw.liu
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            print("-----------SoftwareVersionList--------")
            req = SoftwareVersionListRequest()
            
            res = self.__software_version_list_client.call(req)
            
            if not res.success: raise Exception(res.message) 
            data = []
            if res.SoftwareVersionList:
                # 数据不为空
                for softwareVserson_one in res.SoftwareVersionList:
                    # print(softwareVserson_one)
                    softwareVserson_dict = {}
                    softwareVserson_dict["version"] = softwareVserson_one.version
                    softwareVserson_dict["log"] = softwareVserson_one.updateLog
                    softwareVserson_dict["id"] = softwareVserson_one.id
                    
                    
                    data.append(softwareVserson_dict)
            
            ret_dat["data"] = data

            
            log_msg = "[Robot_Parser][SoftwareVersionList]Successful to  get software version list."
            log.LogInfo(self.GetNodeName(),log_msg)

        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)

            rospy.logerr("[Robot_Parser][SoftwareVersionList]Failed to get software version list: {}".format(error))
        finally:
            return ret_dat

    def DowanloadFirmwareVersion(self):
        '''
        下载固件版本
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # print("xiazai")
            req = UpdateCheckRequest()
            req.flag = 1
            res = self.__firmware_update_client.call(req)
            hardware_url ={}
            hardware_url["local_version_url"] = res.update_info.local_version_url_fireware
            ret_dat["data"] = hardware_url
            # print(res)
            # voice = VoiceOutput()                          
            # voice.default_voice_key = "hardware_update_start"
            # self.__voice_pub.publish(voice)
            if not res.success: raise Exception(res.message)      
            log_msg = "[Robot_Parser][DowanloadFirmwareVersion]Successful to download firmware version."
            log.LogInfo(self.GetNodeName(),log_msg)      
        except Exception as error:
            print(error)
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)

            rospy.logerr("[Robot_Parser][DowanloadFirmwareVersion]Failed to download firmware version: {}".format(error))
        finally:
            return ret_dat

    def UpdateFirmwareVersion(self):
        '''
        更新固件版本
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # print("安装")        
            ret_dat["hardware_url"] = self.__firmware_update_state.local_version_url_fireware
            # 改用线程的方式调用固件更新服务，避免请求一直不回的情况
            # firmware_local_version_url = FirmwareUpgradeRequest()
            # firmware_local_version_url.file_path_control = self.__firmware_update_state.local_version_url
            # print(firmware_local_version_url)
            # self.__firmware_upgrade_client.call(firmware_local_version_url)
            
            # voice = VoiceOutput()                           
            # voice.default_voice_key = "hardware_update_ok"
            # self.__voice_pub.publish(voice)           
            download_thread = Thread(target=self.call_firmwareUpgrade)
            download_thread.setDaemon(True)
            download_thread.start()

            ret_dat["msg"] = "updating firmware version"
            log_msg = "[Robot_Parser][UpdateFirmwareVersion]updating firmware version."
            log.LogInfo(self.GetNodeName(),log_msg)          
        except Exception as error:
            # print(error)
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)
            rospy.logerr("[Robot_Parser][UpdateFirmwareVersion]Failed to update firmware version: {}".format(error))
        finally:
            return ret_dat  

    
    def call_firmwareUpgrade(self):
        """
        改用线程的方式调用固件更新服务，避免请求一直不回的情况
        """
        # print("-------call_firmwareUpgrade---------")
        firmware_local_version_url = FirmwareUpgradeRequest()
        firmware_local_version_url.file_path_control = self.__firmware_update_state.local_version_url_fireware
        time.sleep(3)
        # print(firmware_local_version_url)
        log_msg = "[Robot_Parser][call_firmwareUpgrade]updating firmware version."
        log.LogInfo(self.GetNodeName(),log_msg) 
        # log.LogError(self.GetNodeName(),"firmware_local_version_url",log_code=11)
        res = self.__firmware_upgrade_client.call(firmware_local_version_url)

        if not res.success:
            print(res.message) 
            log.LogError(self.GetNodeName(),res.message,11)
            

    def RobotReboot(self,data):
        '''
            工控机重启，不同
        '''
        ret_dat = {};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            voice = True
            if data and "voice" in data and str(data["voice"])=="0":
                voice=False

            if voice:
                # 语音
                voice = VoiceOutput()                           
                voice.default_voice_key = "software_update_ok"
                self.__voice_pub.publish(voice)
            # outage_reboot :1 整机断电重启 、 空或其他其他工控机重启
            if "outage_reboot" in data and data["outage_reboot"] == 1:
                outage_reboot = 1
            else:
                outage_reboot = 0

            # 改用线程的方式重启工控机，避免请求一直不回的情况
            robot_reboot_thread = Thread(target=self.robot_reboot_thread,args=(outage_reboot,))
            robot_reboot_thread.setDaemon(True)
            robot_reboot_thread.start()

            log_msg = "[Robot_Parser]Robot reboot."
            log.LogInfo(self.GetNodeName(),log_msg)
            # os.system("reboot")
        except Exception as error:
            ret_dat["code"]=-1 
            ret_dat["msg"]=str(error)
            # rospy.logerr("[RobotReboot]error: {}".format(error))
        finally:
            return ret_dat

    def robot_reboot_thread(self,outage_reboot):
        '''
        重启工控机
        ''' 
        if outage_reboot == 1:
            self.__reboot_client.call(TriggerRequest()) # 工控机重启
        else:
            time.sleep(3)
            os.system("reboot")

    def CalibLidar(self, data):
        '''
            激光雷达标定
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")

            # 1-开启标定，2-完成标定 3-更新标定
            cmd = data["cmd"] 
            is_start = False
            if 1 == cmd: 
                is_start = True
                self.__lidar_calib_mode = 1
            elif 2 == cmd:
                is_start = False
            elif 3 == cmd:  #等待激光雷达标定更新srv实现后，更新
                is_start = True
                self.__lidar_calib_mode = 3
            else:
                raise Exception("cmd value error!")
            
            if 1 == self.__lidar_calib_mode:
                res = self.__lidar_calib_client.call(is_start)
                if not res.success: raise Exception("Service</lidar_calibration> error: {}".format(res.message))
            elif 3 == self.__lidar_calib_mode:
                res = self.__lidar_calib_update_client.call(is_start)
                if not res.success: raise Exception("Service</lidar_calibration_update> error: {}".format(res.message))
            log_msg = "[Robot_Parser]Lidar calibration: {}.".format(is_start)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Lidar calibration error: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat
        

    def LidarInstallCheck(self):
        '''
        雷达安装标定
        订阅 话题 /lidar_chec 信息,反馈给APP
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
           
            # print(req)
        
            ret_dat["data"] = self.__lidar_check_info
            log_msg = "[Robot_Parser][LidarInstallCheck]Lidar check info."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][LidarInstallCheck]Lidar check info error: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        

    def LidarInstallCheckBegin(self):
        '''
        雷达安装标定开启 
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
           
            req = Lidar2dCalibrationRequest()
            req.start = True
            
            res = self.__lidar2d_calibration_server_client.call(req)

            if not res.is_success: raise Exception("Service</lidar_calibration_start> error!")

            log_msg = "[Robot_Parser][LidarInstallCheckBegin]Lidar calibration start."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][LidarInstallCheckBegin]Lidar calibration start error: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    def LidarInstallCheckStop(self):
        '''
        雷达安装标定
        结束后关闭服务 
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
           
            req = Lidar2dCalibrationRequest()
            req.start = False
            
            res = self.__lidar2d_calibration_server_client.call(req)

            if not res.is_success: raise Exception("Service</lidar_calibration_start> error!")

            log_msg = "[Robot_Parser][LidarInstallCheckStop]Lidar calibration stop."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][LidarInstallCheckStop]Lidar calibration stop error: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        


    def ImuInstallCheckStart(self):
        '''
        IMU安装标定开启
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            req = SetBoolRequest()
            req.data = True
            res = self.__imu_calibration_server_client.call(req)

            
            if not res.success: raise Exception("Service</imu_calibration_server> error: {}".format(res.message))
            log_msg = "[Robot_Parser][ImuInstallCheckStart]Imu calibration server start."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][ImuInstallCheckStart]Imu calibration server start: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        

    def ImuInstallCheckStop(self):
        '''
        IMU安装标定结束 
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            req = SetBoolRequest()
            req.data = False
            res = self.__imu_calibration_server_client.call(req)

            
            if not res.success: raise Exception("Service</imu_calibration_server> error: {}".format(res.message))
            log_msg = "[Robot_Parser][ImuInstallCheckStop]Imu calibration server stop."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][ImuInstallCheckStop]Imu calibration server stop: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    def ImuInstallCheckData(self):
        '''
        IMU安装标定数据 
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
           
            # print(req)
        
            ret_dat["data"] = self.__imu_calibration_status_info
            log_msg = "[Robot_Parser][DepthInstallCheckDate]camera check info."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][DepthInstallCheckDate]camera check info error: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    def ImuInstallCheckStartSlamA(self):
        '''
        IMU SlamA安装标定开启
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            req = SetBoolRequest()
            req.data = True
            res = self.__imu_calibration_server_slam_a_client.call(req)

            
            if not res.success: raise Exception("Service</imu_calibration_server_slam_a> error: {}".format(res.message))
            log_msg = "[Robot_Parser][ImuInstallCheckStart]Imu calibration server start."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][ImuInstallCheckStart]Imu calibration server slam a start: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat   
        
    def ImuInstallCheckDataSlamA(self):
        '''
        IMU SlamA安装标定数据 查询
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
           
            # print(req)
        
            ret_dat["data"] = copy.deepcopy(self.__imu_calibration_status_info)
            
            if self.__imu_calibration_status_info["calibrate_result"] == "gyro_success"\
                or self.__imu_calibration_status_info["calibrate_result"] == "bias_success"\
                or self.__imu_calibration_status_info["calibrate_result"] == "bias_failed"\
                or self.__imu_calibration_status_info["calibrate_result"] == "gyro_failed":
                self.__imu_calibration_status_info["calibrate_result"] = ""
            log_msg = "[Robot_Parser][DepthInstallCheckDate]camera check info."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][DepthInstallCheckDate]camera check info error: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat


    def SetFactoryInit(self, data):
        '''
            出厂设置
            {u'function': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], u'device_type_child': u'SW55A_AA', u'frp_host': u'frp', u'device_type': u'SW55', u'device_batch': u'202211'}
            {u'function': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], u'device_type_child': u'SW80P_AA', u'frp_host': u'frp', u'device_type': u'SW80 Pro', u'device_batch': u'26'}
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            print(">>>>>>>>>>>>>>>>>>>>>>>>")
            print(data)
            if not data : raise Exception("Request data error!")
            
            if not data["device_type"]: raise Exception("device_type is empty!")
            device_type = data["device_type"]
            
            if not data["device_batch"]: raise Exception("device_batch is empty!")
            device_batch = data["device_batch"]
            if not data["frp_host"]: raise Exception("frp_host is empty!")
            frp_host = data["frp_host"]
            if not data["device_type_child"]: raise Exception("device_type_child is empty!")
            device_type_child = data["device_type_child"]
   
            function = data["function"]
            if len(function) != 16: raise Exception("function length error!")

            req = GenerateRobotSNRequest()
            req.robot_type = device_type
            req.robot_type_sw = device_type_child
            req.robot_batch = device_batch
            req.frp_host = frp_host
            
            for value in function:
                req.robot_fun.append(value)
            res = self.__generate_robot_sn_client.call(req)
            if not res.success: raise Exception("Service</generate_robot_sn> error: {}".format(res.message))
            ret_dat["data"] = res.robot_sn

            log_msg = "[Robot_Parser]Generate robot SN: {}.".format(res.robot_sn)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            #ret_dat["ret"] = 'fail'
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to generate robot SN: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat

    def GetFactorySnInfo(self):
        '''
            序列号信息
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not self.__factory_sn_info: raise Exception("factory_sn_info is empty!")
            ret_dat.update(self.__factory_sn_info)
            log_msg = "[Robot_Parser]Successful to get factory sn infomation."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to get factory sn infomation: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
    
    def SetFactoryDeliver(self):
        '''
            发货设置
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            req = EmptyRequest()
            #res = self.__robot_reset_client.call(req)
            self.__robot_reset_client.call(req)
            #if not res.success: raise Exception("Service</robot_reset> error: {}".format(res.message))
        
            log_msg = "[Robot_Parser]Successful to reset robot."
            log.LogInfo(self.GetNodeName(),log_msg)
            ## 用户重新设置后，机器自动重启
            os.system("reboot")
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to reset robot."
            log.LogError(self.GetNodeName(),err_msg)
        finally:
            return ret_dat
            
    def FactoryUpdateType(self, data):
        '''
            修改机器型号 20230804-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")
            
            if not data["type"]: raise Exception("type is empty!")
            device_type = data["type"]      # 新设备型号名称
            if not data["device_type_child"]: raise Exception("device_type_child is empty!")
            device_type_child = data["device_type_child"]      # 新设备型号名称
            
            if not data["authorizationCode"]: raise Exception("authorizationCode is empty!")
            authorizationCode = data["authorizationCode"] # 云平台生成的授权码
            

            req = UpdateRobotSNRequest()
            req.robot_type = device_type
            req.authorizationCode = authorizationCode
            req.robot_type_sw = device_type_child
            
            res = self.__robot_sn_change_server_client.call(req)
            if not res.success: raise Exception("Service</robot_sn_change> error: {}".format(res.message))
            
            if self.__factory_sn_info.has_key("robot_sn"):
                robot_sn = self.__factory_sn_info["robot_sn"]
            log_msg = "[Robot_Parser]update robot SN: {}.".format(robot_sn)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:

            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to update robot SN: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            # rospy.logerr(err_msg)
        finally:
            return ret_dat
        

        
    def RvizPoints(self, data):
        '''
            rviz点集
            2023.2.1 --dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")
            
            if not data["topic_name"]: raise Exception("topic_name is empty!")
                  
            if not data["points"]: raise Exception("points is empty!")
            
            if not data["showDirect"]: raise Exception("showDirect is empty!")
                      
            init_path = Path()

            for rviz_point in data["points"]:

                init_path.header.stamp = rospy.Time.now()
                init_path.header.frame_id = "map"
                
                rviz_point_pose = PoseStamped()
                rviz_point_pose.header.frame_id = "map"

                rviz_point_x = float(rviz_point["x"])
                rviz_point_y = float(rviz_point["y"])
                rviz_point_yaw = float(rviz_point["yaw"])

                rviz_point_pose.pose.position.x = rviz_point_x
                rviz_point_pose.pose.position.y = rviz_point_y
                rviz_point_pose.pose.position.z = 0.
                q = tf.transformations.quaternion_from_euler(0, 0, rviz_point_yaw)
                rviz_point_pose.pose.orientation.x = q[0]
                rviz_point_pose.pose.orientation.y = q[1]
                rviz_point_pose.pose.orientation.z = q[2]
                rviz_point_pose.pose.orientation.w = q[3]

                init_path.poses.append(rviz_point_pose)
                #发布消息
                self.__rviz_points_pub.publish(init_path)

            log_msg = "[Robot_Parser]Successful to set rviz points: {}.".format(init_path)
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to set rviz points: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
            rospy.logerr(err_msg)
        finally:
            return ret_dat
        
    
    def CurrentWarnList(self):
        '''
            当前的报警记录 20230515-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            
            
                      
            # print(self.__curr_alarm_events_info.event_array)
            data = []
            for event_array_one in self.__curr_alarm_events_info.event_array:
                # print(event_array_one)
                array_one = {}
                array_one["code"] = event_array_one.event_code
                array_one["level"] = event_array_one.event_level
                array_one["module"] = event_array_one.node_name
                array_one["message"] = event_array_one.event_code_str
                data.append(array_one)

            ret_dat["data"] = data



            log_msg = "[Robot_Parser][CurrentWarnList]Successful to return current alarn events info."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][CurrentWarnList]Failed to return current alarn events info.: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
        finally:
            return ret_dat
        




    def BagRecordStart(self,data):
        '''
            开始录制bag 20230912-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            # print(data)

            req = BagRecordRequest()

            req.state = BagRecordRequest().RECORD_START
            if "options" in data:
                for item in data["options"]:
                    if item == BagRecordRequest.options_hardware:
                        req.options.append(item)
                    if item == BagRecordRequest.options_sensors:
                        req.options.append(item)
                    if item == BagRecordRequest.options_map:
                        req.options.append(item)
                    if item == BagRecordRequest.options_docker:
                        req.options.append(item)
                    if item == BagRecordRequest.options_task:
                        req.options.append(item)
                    
            # print(req)        

            self.__bag_recore_server_client(req)
        
            log_msg = "[Robot_Parser][BagRecordStart]successfully"
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][BagRecordStart]Failed to start record bag : {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
        finally:
            return ret_dat


    def BagRecordEnd(self):
        '''
            结束录制bag 20230912-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            
            req = BagRecordRequest()

            req.state = BagRecordRequest().RECORD_FINISHED
               

            self.__bag_recore_server_client(req)
        
            log_msg = "[Robot_Parser][BagRecordEnd]successfully"
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][BagRecordEnd]Failed to end record bag.: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
        finally:
            return ret_dat
        

    def BagRecordStatus(self):
        '''
            获取bag状态 20230912-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            
            data = {}
            data["bag_name"] = self.__bag_status_msg.bag_name
            data["bag_size"] = self.__bag_status_msg.bag_size
            data["disk_size"] = self.__bag_status_msg.disk_size
            data["disk_free_size"] = self.__bag_status_msg.disk_free_size
            data["bag_recording"] = self.ConvertBool(self.__bag_status_msg.bag_recording)

            ret_dat["data"] = data

            # log_msg = "[Robot_Parser][BagRecordStatus]successfuly"
            # log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser][BagRecordStatus]Failed to get  bag status: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            
        finally:
            return ret_dat
    
    def ConvertBool(self, bool):
        """
        将布尔值转换为整数类型（1或0）。
        
        Args:
            bool (bool): 需要转换的布尔值。
        
        Returns:
            int: 转换后的整数类型，如果bool为True则返回1，否则返回0。
        
        """
        return 1 if bool else 0
    


#---------------------------------#
# ft.wang 20240606接手
#---------------------------------#
    
    def GetCleandeviceWithMemoryParams(self):
        '''
            外设控制设置默认参数列表 获取
        '''
        response = {"msg": "success", "code": 0, "data": []}
        try:
         
            cleandevice_config_params = rospy.get_param("/cleandevice_mode_config",{})
            cleandevice_config_default_params = rospy.get_param("/cleandevice_config_default",{})
            language = rospy.get_param("/language",0)   #0 中文 1 英文 2 繁体中文 3 西班牙语 4 法语 5 日语 6 韩语 7 德语   

            ## 优先添加默认值
            for item_name in cleandevice_config_default_params["default_mode_list"]:
                if item_name in cleandevice_config_params:
                    # 切换语言
                    translation_list = cleandevice_config_default_params["translation"][item_name]
                    if language < len(translation_list):
                        cleandevice_config_params[item_name]["item_name"] = translation_list[language]  
                    # 添加数据
                    response["data"].append(cleandevice_config_params[item_name])
                    # 删除源数据
                    del cleandevice_config_params[item_name]

            ## 在添加剩余的参数
            for item_name, item_value in cleandevice_config_params.items():
                if item_name != 'map':
                    response["data"].append(item_value)
            
            # 按创建时间排序
            response["data"] = sorted(response["data"], key=lambda x: x["add_time"])
                       
            log_msg = "[Robot_Parser]Successful to get cleandevice with memory parameters."
            log.LogInfo(self.GetNodeName(),log_msg)

        except Exception as error:
            response["code"] = -1
            response["msg"] = str(error)
            err_msg = "[Robot_Parser] Failed to get cleandevice with memory parameters: {}".format(error)
            log.LogError(self.GetNodeName(), err_msg)

        return response
        
    def SetCleandeviceWithMemoryParams(self,data):
        '''
            外设控制设置默认参数 设置
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data: raise Exception("Request data error!")
            # default_item_name = data.get("default_memory_name")
            item_key = data.get("item_key")

            # 校验参数
            cleandevice_config_params = rospy.get_param("/cleandevice_mode_config", {})
            if not cleandevice_config_params:
                raise Exception("Cleandevice config params are empty!")

            # 更新默认值
            for key, value in cleandevice_config_params.items():
                if key != 'map':
                    # if item_value["item_name"] == default_item_name:
                    if item_key == key:
                        value["is_default"] = 1
                    else:
                        value["is_default"] = 0
            # print(cleandevice_config_params)

            # 从字典类型的数据中设置ROS参数。
            rospy.set_param('cleandevice_mode_config', cleandevice_config_params)

            # 通知其他节点参数变更
            update_data = StringArray()
            update_data.data.append('/cleandevice_mode_config')
            self.__robot_param_change.publish(update_data)

            log_msg = "[Robot_Parser]Successful to set cleandevice with memory parameters."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to set cleandevice with memory parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

        return ret_dat

    def DeleteCleandeviceWithMemoryParams(self,data):
        '''
            外设控制设置默认参数 删除
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data: raise Exception("Request data error!")
            # delete_item_name = data.get("item_name")
            item_key = data.get("item_key")

            # 校验参数
            cleandevice_config_params = rospy.get_param("/cleandevice_mode_config", {})
            if not cleandevice_config_params:
                raise Exception("Cleandevice config params are empty!")
            
            # 获取键, 确认map数据要匹配
            # item_key = None
            # for key, value in cleandevice_config_params["map"].items():
            #     if value == delete_item_name:
            #         item_key = key   
            # if not item_key:
            #     raise Exception("The request mode key does not exist!")
            print(item_key)
            del cleandevice_config_params["map"][item_key]
            # 删除
            if item_key in cleandevice_config_params:
                del cleandevice_config_params[item_key]
    
            # print(cleandevice_config_params)
            # 从字典类型的数据中设置ROS参数。
            rospy.set_param('cleandevice_mode_config', cleandevice_config_params)

            # 通知其他节点参数变更
            update_data = StringArray()
            update_data.data.append('/cleandevice_mode_config')
            self.__robot_param_change.publish(update_data)
            
            log_msg = "[Robot_Parser]Successful to delete cleandevice with memory parameters."
            log.LogInfo(self.GetNodeName(),log_msg)
        
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to delete cleandevice with memory parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

        return ret_dat


    def AddCleandeviceWithMemoryParams(self,data):
        '''
            外设控制设置默认参数 添加
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data: raise Exception("Request data error!")
            add_item_name = data.get("item_name")
            is_default = data.get("is_default")

            cleandevice_config_params = rospy.get_param("/cleandevice_mode_config", {})
            if not cleandevice_config_params:
                raise Exception("Cleandevice config params are empty!")

            # 新建map映射
            now_time = rospy.get_rostime()
            new_key = "mode_{}_{}".format(now_time.secs, now_time.nsecs)
            
            if "map" in cleandevice_config_params:
                for item_name, item_value in cleandevice_config_params["map"].items():
                    if str(item_value) == add_item_name:
                        raise Exception("The request mode key already exists!")
                cleandevice_config_params["map"][new_key] = add_item_name
            
            # 新增配置
            cleandevice_config_params[new_key] = data
            cleandevice_config_params[new_key]["add_time"] = now_time.secs
            cleandevice_config_params[new_key]["item_key"] = new_key

            # 遍历字典，更新is_defualt
            if is_default:
                for item_name, item_value in cleandevice_config_params.items():
                    if item_name != 'map':
                        if item_value["is_default"] and str(item_value["item_name"]) != add_item_name:
                            item_value["is_default"] = 0
            # print(cleandevice_config_params)

            # 当主刷挡位置为低（21）、中（22）、高（23）、关闭（8）时，高度自定义等级控制（203_）置为0
            if "id_22" in data and data.get("id_22") in ["21","22","23","8"]:
                data["id_203"] = 0
           
            # 从字典类型的数据中设置ROS参数。
            rospy.set_param('cleandevice_mode_config', cleandevice_config_params)

            # 通知其他节点参数变更
            update_data = StringArray()
            update_data.data.append('/cleandevice_mode_config')
            self.__robot_param_change.publish(update_data)
          
            log_msg = "[Robot_Parser]Successful to add cleandevice with memory parameters."
            log.LogInfo(self.GetNodeName(),log_msg)
        
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to add cleandevice with memory parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
     
        return ret_dat

    def ChangeCleandeviceWithMemoryParams(self,data):
        '''
            外设控制设置默认参数 修改
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data: raise Exception("Request data error!")
            item_name = data.get("item_name")
            is_default = data.get("is_default")

            cleandevice_config_params = rospy.get_param("/cleandevice_mode_config", {})
            if not cleandevice_config_params:
                raise Exception("Cleandevice config params are empty!")

            # 获取键, 确认map数据要匹配
            item_key = None
            for key, value in cleandevice_config_params["map"].items():
                if str(value) == item_name:
                    item_key = key  
            # 再次确认是否在默认列表中
            cleandevice_config_default_params = rospy.get_param("/cleandevice_config_default",{})
            for default_mode, value in cleandevice_config_default_params["translation"].items():
                if item_name in value:
                    item_key = default_mode  
            print("--> [ChangeCleandeviceWithMemoryParams] item_key: {}".format(item_key))     
            if not item_key:
                raise Exception("The request mode key does not exist!")
            # 修复多国语言配置时，同步更改item_name
            cleandevice_config_params["map"][item_key] = item_name
            
            # 更新配置
            cleandevice_config_params[item_key] = data

            # 遍历字典，更新is_defualt
            if is_default:
                for key, value in cleandevice_config_params.items():
                    if key != 'map':
                        if value["is_default"] and str(value["item_name"]) != item_name:
                            value["is_default"] = 0
            # print(cleandevice_config_params)           

            # 当主刷挡位置为低（21）、中（22）、高（23）、关闭（8）时，高度自定义等级控制（203_）置为0
            if "id_22" in data and data.get("id_22") in ["21","22","23","8"]:
                data["id_203"] = 0
                
            # 从字典类型的数据中设置ROS参数。
            rospy.set_param('cleandevice_mode_config', cleandevice_config_params)

            # 通知其他节点参数变更
            update_data = StringArray()
            update_data.data.append('/cleandevice_mode_config')
            self.__robot_param_change.publish(update_data)

        
            log_msg = "[Robot_Parser]Successful to change cleandevice with memory parameters."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to change cleandevice with memory parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

        return ret_dat    

    def CleandeviceParamsReset(self, data):
        '''外设控制参数（出厂参数）重置接口'''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data: raise Exception("Request data error!")
            # item_name = data.get("item_name")  
            item_key = data.get("item_key")

            # 校验参数
            cleandevice_config_params = rospy.get_param("/cleandevice_mode_config", {})
            cleandevice_config_default_params = rospy.get_param("/cleandevice_config_default", {})
            if not cleandevice_config_params or "map" not in cleandevice_config_params:
                raise Exception("cleandevice_mode_config params are empty!")
            if not cleandevice_config_default_params or "map" not in cleandevice_config_default_params:
                raise Exception("cleandevice_config_default params are empty!")
            
            # 获取默认参数的键
            # item_key = None
            # for key, value in cleandevice_config_default_params["map"].items():
            #     if value == item_name:
            #         item_key = key
            # if not item_key:
            #     raise Exception("The request mode key does not exist!")
            
            # 恢复为默认值
            if "mode" not in item_key:
                raise Exception("The item_key does not contain the mode content. item_key:{}".format(item_key))
            
            if item_key in cleandevice_config_params:
                cleandevice_config_params[item_key] = cleandevice_config_default_params[item_key]

            # 从字典类型的数据中设置ROS参数。
            rospy.set_param('cleandevice_mode_config', cleandevice_config_params)

            # 通知其他节点参数变更
            update_data = StringArray()
            update_data.data.append('/cleandevice_mode_config')
            self.__robot_param_change.publish(update_data)

            log_msg = "[Robot_Parser]Successful to change cleandevice with memory parameters."
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to change cleandevice with memory parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

        return ret_dat  


    #-----------------------------------#
    #******#  Interface function #******# 
    #-----------------------------------#
    def SetRobotParams(self, data):
        """
        设置机器人参数
        
        Args:
            data (dict): 包含需要设置的参数及其值的字典
            {u'language': 1}
            {u'night_mode': {u'start_time': u'14-00', u'enable': 1, u'end_time': u'14-27'}}
            {u'working_voice': {u'data_en': u'', u'enable': 1, u'data': u'hello', u'frequency': 30}}
            {u'warn_state': {u'alarm_to_user': 0}} 提醒
            {u'warn_state': {u'alltask_finished': 1}} 提醒
            {u'cleantask': {u'all_finished': 1}} 任务结束行为
            {u'cleantask': {u'all_finished': 0}}
            {u'nav': {u'dynamicObstaclewaitingTime': 6}} 避障等待时长
            {u'speed_level': 2} 方控移动速度
            {u'p2p_speed_level': 2} 导航移动速度
            {u'clean_width': {u'sweep_width': 0.25, u'path_width': 0.201}} 清扫宽度
            {u'docker': {u'docking_force_push_vel': 0.08, u'release_docker_time': 2.0, u'docker_distance': 0.8, u'docking_force_push_time': 3.0}} 回桩设置
            {u'elevlator': {u'yuji_lora_id': u'kkmm', u'yuji_lift_id': u'fcxxdd', u'yuji_place_id': u'ccccc', u'platform': u'A'}} 梯控方案设置
        Returns:
            dict: 包含操作结果的字典，其中msg字段表示结果信息，code字段表示结果状态码（0表示成功，-1表示失败）
        
        """
        # print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>")
        # print(data)
        response_data = {"msg": "success", "code": 0}

        # 参数验证
        if not data:
            response_data["code"] = -1
            response_data["msg"] = "Request data error!"
            log.LogError(self.GetNodeName(), "[Robot_Parser]Failed to set robot parameters: {}".format(response_data["msg"]))
            return response_data

        try:
            # 从字典类型的数据中设置ROS参数。
            self.SetParams(data)
            
            # 通知其他节点参数变更
            self.PubRobotParamChangeTopic(data)
            
            if 'elevlator' in data:
                self.Send_cloudPlatform_bindElevator()
                                  
        except Exception as error:
            response_data["code"] = -1
            response_data["msg"] = str(error)
            log.LogError(self.GetNodeName(), "[Robot_Parser]Failed to set robot parameters: {}".format(error))

        finally:
            return response_data  
        
   
    def SetParams(self, data, prefix=""):
        """
        从字典类型的数据中设置ROS参数。
        
        Args:
            data (dict): 包含参数键值对的字典。
            prefix (str, optional): 参数的前缀。默认为空字符串。
        
        Returns:
            None
        
        """
        # 深度相机数据格式兼容
        if "depth_camera_config" in data:
            for item,value in data["depth_camera_config"].items():
                new_data = value["install_pose"]
                install_pose_key = ["x", "y", "z", "roll", "pitch", "yaw"]
                install_pose = []
                for key in install_pose_key:
                    if key in install_pose_key:
                        install_pose.append(new_data[key])
                    else:
                        install_pose.append(0.0)
                data["depth_camera_config"][item]["install_pose"] = install_pose
        
        print("SetParams", data)

        for key, value in data.items():
            # 如果值是字典，则递归设置参数
            if isinstance(value, dict):
                self.SetParams(value, prefix + "/" + key)
            else:
                # 构建参数键
                param_key = prefix + "/" + key if prefix else key
                # 使用 rospy.set_param() 设置参数到 ROS Parameter Server
                # print(param_key, value)
                rospy.set_param(param_key, value)

    def getDepthCameraConfig(self):
        '''获取深度相机配置参数'''
        res_data = {}
        depth_camera_config = rospy.get_param("/depth_camera_config", {})
        # print(depth_camera_config)
        for key, value in depth_camera_config.items():
            print(key, value)
            item_config = {}
            item_config["install_pose"] = {
                "x": value["install_pose"][0],
                "y": value["install_pose"][1],
                "z": value["install_pose"][2],
                "roll": value["install_pose"][3],
                "pitch": value["install_pose"][4],
                "yaw": value["install_pose"][5],
            }
            res_data[key] = item_config
        return res_data

    def getLidarConfig(self):
        '''获取雷达配置参数'''
        res_data = {}
        lidar_config = rospy.get_param("/lidar_2d_config", {})
        for key, value in lidar_config.items():
            print(key, value)
            item_config = {}
            item_config["display_mode"] = value["display_mode"]
            res_data[key] = item_config
        return res_data      

    def getDepth2laserConfig(self):
        '''获取深度转雷达配置参数'''
        res_data = {}
        depth2laser_config = rospy.get_param("/depth2laser_combined_node", {})
        res_data['display_mode'] = depth2laser_config.get("display_mode",True)
        return res_data 
        

    def GetRobotParams(self):
        '''
            用户参数获取
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            
            # 包含相机相关参数的字典，包括相机数量、相机列表、depth2laser参数等
            ret_dat.update(self.GetDepthCameraParams())
            # 获取充电桩相关参数
            ret_dat.update(self.GetDockerParams())
            # 获取清扫宽度参数
            ret_dat.update(self.GetCleanWidthParams())
            # 获取夜间模式参数
            ret_dat.update(self.GetNightModeParams())
            # 获取报警提醒参数
            ret_dat.update(self.GetWarnStateParams())
            # 梯控相关参数
            ret_dat.update(self.GetElevlatorParams())
            # 获取充电相关参数
            ret_dat.update(self.GetChargeParams())
            # 获取工作词参数
            ret_dat.update(self.GetWorkingVoiceParams())
            # nav避障等待时长
            ret_dat.update(self.GetNavParams())
            # 任务结束后行为
            ret_dat.update(self.GetCleanTaskParams())
            # 获取当前时区 20230728-dw.liu
            ret_dat["timeZoneId"] = self.GetTimezone()
            # 获取音量
            ret_dat["volume"] = rospy.get_param('/volume', self.__volume)
            # 语言
            ret_dat["language"] =  rospy.get_param("/language",0) #0 中文 1 英文 2 繁体中文 3 西班牙语 4 法语 5 日语 6 韩语 7 德语 
            # 平板（操作杆）抬起状态 
            ret_dat["pad_raise_state"] = rospy.get_param("/pad_raise_state",0)
            ret_dat["sw55_pad_state"] = rospy.get_param("/sw55_pad_state",0)
            
            # 机器人外接圆半径 单位米
            ret_dat["robot_expansion_distance"] = rospy.get_param("robot_radius",0.35)
            # pad控制机器人的速度等级,方控移动速度
            ret_dat["speed_level"] = rospy.get_param("/speed_level",1)
            # 用于用户配置任务结束是否回桩充电
            # ret_dat["alltask_finished_gohome_user"] = rospy.get_param("/cleantask",{}).get("all_finished",1) 
            # 导航移动速度 
            ret_dat["p2p_speed_level"] = rospy.get_param("/p2p_speed_level",2)

            ret_dat.update(self.GetCleanTaskParams())

            ret_dat["device_control_map"] = self.getDeviceControlMap()
            ret_dat["depth_camera_config"] = self.getDepthCameraConfig()
            ret_dat["depth_camera_calibration"] = rospy.get_param("/depth_camera_calibration", {})
            ret_dat["timertask"] = rospy.get_param("/timertask", {})
            ret_dat["lidar_2d_config"] = self.getLidarConfig()
            ret_dat["depth2laser_combined_node"] = self.getDepth2laserConfig()
            

        except Exception as error:

            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)

            err_msg = "[Robot_Parser]Failed to get robot parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)

        return ret_dat


    def GetDepthCameraParams(self):
        """
        获取相机相关参数
        
        Args:
            无
        
        Returns:
            dict: 包含相机相关参数的字典，包括相机数量、相机列表、depth2laser参数等
        
        """
        ret_dat = {}
        try:
        
            # 语言
            language_type = rospy.get_param("/language", 0)  #0 中文 1 英文 2 繁体中文 3 西班牙语 4 法语 5 日语 6 韩语 7 德语 
            language_dict = {0: "cn", 1: "en", 2: "tc", 3: "es", 4: "fr", 5: "ja", 6: "ko", 7: "de"}
            lang_key = language_dict.get(language_type, "cn")
                
            # 深度相机传感器数量
            depth_camera_nums = rospy.get_param("/depth_camera_nums",2)
            
            # 获取深度相机配置参数
            depth_camera_params = rospy.get_param("/depth_camera_config", {})
            
            # 相机列表
            camera_lists = []
            # 获取depth2laser参数   
            depth2laser_params = {}

                        
            for camera_name, camera_param in depth_camera_params.items():
                
                node_params = {}
                
                # 相机列表
                camera_one = {
                    "id": camera_param.get("id", 0x01),
                    "name": camera_param.get(lang_key, "Unknown")
                }
                camera_lists.append(camera_one)
                
                # # 相机安装位姿参数
                # install_pose = camera_param.get(
                #     "install_pose", [0.0, 0.0, 0.13, 0.0, -47.0, 50.0]
                # )
                # node_params['install_pose'] = {
                #     'z': install_pose[2],
                #     'roll': install_pose[3],
                #     'pitch': install_pose[4],
                #     'yaw': install_pose[5]
                # } if len(install_pose) == 6 else {
                #     'z': 0.13,
                #     'roll': 0.0,
                #     'pitch': -47.0,
                #     'yaw': 50.0
                # }
                
                depth2laser_node_prefix = "depth2laser_node_{}".format(camera_param.get('id', 0x01))
                depth2laser_node_params = rospy.get_param(depth2laser_node_prefix, {})
                limit = depth2laser_node_params.get("limit", {})
                node_params['limit'] = {
                    'top': limit.get('top', 0.65),
                    'bottom': limit.get('bottom', 0.03)
                }
                
                depth2laser_params[depth2laser_node_prefix] = node_params
                
                # camera_01,camera_02,camera_03,camera_04,camera_05
                camera_prefix = "camera_0{}".format(camera_param.get('id', 0x01))
                ret_dat[camera_prefix] = rospy.get_param(camera_prefix,{})
            
            # 获取depth2laser参数  
            ret_dat.update(depth2laser_params)
            # 相机数量
            ret_dat["camera_nums"] = depth_camera_nums
            # 相机列表
            ret_dat["camera_list"]  = camera_lists     
        
            return ret_dat 
        except Exception as error:
            err_msg = "Error fetching depth camera parameters: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
            return {}  


    def DepthCameraCalibration(self, data):
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        ret_dat["data"] = {}
        try:
            if not data or not data["camera_id"]: 
                raise Exception("Request data error!")
            # 解析ID
            id = str(data["camera_id"])
            camera_id = "ALL"
            if id == "1":
                camera_id = CameraCalibrationRequest.CAMERA_01     #"camera_01"
            if id == "2":
                camera_id = CameraCalibrationRequest.CAMERA_02     #"camera_02"
            if id == "3":
                camera_id = CameraCalibrationRequest.CAMERA_03      #"camera_03"
            if id == "4":
                camera_id = CameraCalibrationRequest.CAMERA_04      #"camera_04"
            if id == "5":
                camera_id = CameraCalibrationRequest.CAMERA_05      #"camera_05"
            if id == "100":
                camera_id = "ALL"
            
            # 执行相应服务
            if camera_id == "ALL":
                req = SetBoolRequest(data=True)
                res = self.__depth_camera_combined_calibrate_client.call(req)
                if not res.success: 
                    raise Exception(res.message)
            else:
                req = CameraCalibrationRequest()
                req.serial = camera_id
                req.data = True
                res = self.__depth_camera_calibrate_client.call(req)
                if not res.success: 
                    raise Exception(res.message)
                if len(res.install_pose) == 6:
                    install_pose = {}
                    install_pose["x"] = res.install_pose[0]
                    install_pose["y"] = res.install_pose[1]
                    install_pose["z"] = res.install_pose[2]
                    install_pose["roll_deg"] = res.install_pose[3]
                    install_pose["pitch_deg"] = res.install_pose[4]
                    install_pose["yaw_deg"] = res.install_pose[5]
                    ret_dat["data"] = install_pose
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)
            err_msg = "[Robot_Parser][DepthCameraCalibration]Camera calibration error: {}".format(error)
            log.LogError(self.GetNodeName(),err_msg)
        finally:
            return ret_dat


    def RobotTypeList(self):
        '''
            机器人型号列表 20240508-dw.liu
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            robot_type_list = []
            
            # robot_type_list = ["SW55","SW55 A","SW55L","SW55 Cruiser","SW55 Spary","KT60 Spary","SW80 Pro"]
            
            product_robot_type_params = rospy.get_param("/product_robot_type",{})
            if product_robot_type_params:
                for type,params in product_robot_type_params.items():
                    robot_type_dict = {
                        "type": type,
                        "childs": params.get("type_list_sw",[])
                    }
                    robot_type_list.append(robot_type_dict)
            
            # ret_robot_type_list = []
            # for robot_type in robot_type_list:
            #     if robot_type in product_robot_type_params:
            #         robot_type_dict = {
            #             "type": robot_type,
            #             "childs": product_robot_type_params.get(robot_type,{}).get("type_list_sw",[])
            #         }
            #         ret_robot_type_list.append(robot_type_dict)
            ret_dat["data"] = robot_type_list
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)  
        return ret_dat


    def BagRecordTrigger(self, data):
        '''
            bag录制触发机制
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")
            if "trigger_type" not in data: 
                raise Exception("trigger_type is empty!")
            trigger_type = data["trigger_type"].encode('utf-8')
            client_data = BagsRecordRequest()
            client_data.node_name = "http_node"
            client_data.description = trigger_type
            resp = self.__bags_record_client.call(client_data)
            if not resp.response.success:
                raise Exception(resp.response.message)
            ret_dat["bag_filename"] = resp.bag_filename
        except Exception as error:
            ret_dat["code"] = -1
            ret_dat["msg"] = str(error)  
        return ret_dat


    def MapBackupSync(self,data):
        '''
            地图数据备份 - 云同步
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        client_data = CloudSyncFileManagerRequest()
        try:
            if not data: raise Exception("Request data error!")
            robot_sn = data["robot_sn"].encode('utf-8')
            cmd = data["cmd"]
            client_data.sync_file.file_type = client_data.sync_file.BACKUP_FILE
            client_data.option = int(cmd)      
            # 客户端请求
            res_data = self.__cloudsync_file_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            ret_dat["upload_file_name"] = res_data.sync_file.file_name
            ret_dat["upload_file_size"] = res_data.sync_file.file_size
            ret_dat["upload_progress"] = res_data.sync_file.sync_percentage
            ret_dat["upload_speed"] = res_data.sync_file.sync_speed
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MapBackupSync] error: {}".format(error))        
        finally:
            return ret_dat 

    def FileType2Str(self, file_type):
        sync_file = SyncFile()
        if file_type == sync_file.ROS_BAG:
            return "ros_bags"
        if file_type == sync_file.NAV_LOG:
            return "nav_log"
        return ""

    def FileCloudSync(self,data):
        '''
            云同步 - 文件上传
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        client_data = CloudSyncFileManagerRequest()
        try:
            if not data: raise Exception("Request data error!")
            option = int(data["option"])
            file_type = data["file_type"].encode('utf-8')
            file_name = data["file_name"].encode('utf-8')
            #构造数据
            sync_file = SyncFile()
            sync_file.file_name = file_name
            if file_type == "ros_bags":
                sync_file.file_type = sync_file.ROS_BAG
            if file_type == "nav_log":
                sync_file.file_type = sync_file.NAV_LOG   
            client_data.sync_file = sync_file 
            client_data.option = option  

            # 客户端请求
            res_data = self.__cloudsync_file_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            # 文件状态
            ret_dat["uploading_list"] = list()
            res_sync_file = {
                "file_name" : res_data.sync_file.file_name,
                "file_size" : res_data.sync_file.file_size,
                "file_type" : self.FileType2Str(res_data.sync_file.file_type),
                "create_time" : res_data.sync_file.create_time,
                "upload_progress" : res_data.sync_file.sync_percentage,
                "upload_speed" : res_data.sync_file.sync_speed,
                "upload_status" : "",
                "upload_time" : res_data.sync_file.sync_finished_time
            }
            if res_data.sync_file.sync_status == 1:
                res_sync_file["upload_status"] = "success"
            if res_data.sync_file.sync_status == 2:
                res_sync_file["upload_status"] = "uploading"
            if res_data.sync_file.sync_status == 3:
                res_sync_file["upload_status"] = "failed"
            ret_dat["uploading_list"].append(res_sync_file)
            #文件列表
            ret_dat["file_list"] = list()
            if res_data.sync_file_list:
                for sync_file in res_data.sync_file_list:
                    res_sync_file = {
                        "file_name" : sync_file.file_name,
                        "file_size" : sync_file.file_size,
                        "file_type" : self.FileType2Str(sync_file.file_type),
                        "create_time" : sync_file.create_time,
                        "upload_progress" : sync_file.sync_percentage,
                        "upload_speed" : sync_file.sync_speed,
                        "file_status" : "synchronized" if sync_file.sync_status == 1 else "unknown",
                        "upload_time" : sync_file.sync_finished_time
                    }  

                    ret_dat["file_list"].append(res_sync_file)                
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[FileCloudSync] error: {}".format(error))        
        finally:
            return ret_dat 








    # def DepthInstallCheckStart(self,data):
    #     '''
    #     深度相机安装标定开始 
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     try:
    #         if not data : raise Exception("Request data error!")

    #         serial = data["serial"] # 相机标定序号


    #         req = CameraCalibrationRequest()
    #         req.serial = serial
    #         req.start = True
            
    #         res = self.__camera_calibration_server_client.call(req)

            
    #         if not res.success: raise Exception("Service</camera_calibration_server> error: {}".format(res.message))
    #         log_msg = "[Robot_Parser][DepthInstallCheckStart]Camera calibration start."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
            
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser][DepthInstallCheckStart]Camera calibration start error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #         # rospy.logerr(err_msg)
    #     finally:
    #         return ret_dat
        
    
    # def DepthInstallCheckStop(self,data):
    #     '''
    #     雷达安装标定
    #     结束后关闭服务 
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     try:
    #         if not data : raise Exception("Request data error!")

    #         serial = data["serial"] # 相机标定序号


    #         req = CameraCalibrationRequest()
    #         req.serial = serial
    #         req.start = False
            
    #         res = self.__camera_calibration_server_client.call(req)

            
    #         if not res.success: raise Exception("Service</camera_calibration_server> error: {}".format(res.message))
    #         log_msg = "[Robot_Parser][DepthInstallCheckStop]Camera calibration stop."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
            
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser][DepthInstallCheckStop]Camera calibration stop error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #         # rospy.logerr(err_msg)
    #     finally:
    #         return ret_dat
        
    # def DepthInstallCheckDate(self):
    #     '''
    #     深度相机标定数据
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     try:
           
    #         # print(req)
        
    #         ret_dat["data"] = self.__camera_calibration_status_info
    #         log_msg = "[Robot_Parser][DepthInstallCheckDate]camera check info."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
            
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser][DepthInstallCheckDate]camera check info error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #         # rospy.logerr(err_msg)
    #     finally:
    #         return ret_dat
    
    # def DepthInstall(self,data):
    #     """深度相机现场标定数据"""
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     threads = []
    #     try:
    #         camera_id = data["camera_id"]

    #         req = SetBoolRequest()
    #         req.data = True
    #         res = SetBoolResponse()
            
    #         if "1" in camera_id:
    #             camera_name = 'camera_01'
    #             thread = Thread(target=self.calibration_thread, args=(1,))
    #             thread.setDaemon(True)
    #             threads.append(thread)
    #             thread.start()
            
    #             # 启动一个线程来等待所有校准线程完成
    #             waiter_thread = Thread(target=self.wait_for_completion, args=(threads,))
    #             waiter_thread.setDaemon(True)
    #             waiter_thread.start()
    #             # res = self.__depth2laser_node_1_calibration_server_client(req)
                
    #         elif "2" in camera_id:
    #             camera_name = 'camera_02'
    #             thread = Thread(target=self.calibration_thread, args=(1,))
    #             thread.setDaemon(True)
    #             threads.append(thread)
    #             thread.start()
            
    #             # 启动一个线程来等待所有校准线程完成
    #             waiter_thread = Thread(target=self.wait_for_completion, args=(threads,))
    #             waiter_thread.setDaemon(True)
    #             waiter_thread.start()
                
    #             # res = self.__depth2laser_node_2_calibration_server_client(req)
    #         elif "3" in camera_id:
    #             camera_name = 'camera_03'
    #             thread = Thread(target=self.calibration_thread, args=(1,))
    #             thread.setDaemon(True)
    #             threads.append(thread)
    #             thread.start()
            
    #             # 启动一个线程来等待所有校准线程完成
    #             waiter_thread = Thread(target=self.wait_for_completion, args=(threads,))
    #             waiter_thread.setDaemon(True)
    #             waiter_thread.start()
    #             # res = self.__depth2laser_node_3_calibration_server_client(req)
    #         elif "4" in camera_id:
    #             camera_name = 'camera_04'
    #             thread = Thread(target=self.calibration_thread, args=(1,))
    #             thread.setDaemon(True)
    #             threads.append(thread)
    #             thread.start()
            
    #             # 启动一个线程来等待所有校准线程完成
    #             waiter_thread = Thread(target=self.wait_for_completion, args=(threads,))
    #             waiter_thread.setDaemon(True)
    #             waiter_thread.start()
    #             # res = self.__depth2laser_node_4_calibration_server_client(req)
    #         elif "5" in camera_id:
    #             camera_name = 'camera_05'
    #             thread = Thread(target=self.calibration_thread, args=(1,))
    #             thread.setDaemon(True)
    #             threads.append(thread)
    #             thread.start()
            
    #             # 启动一个线程来等待所有校准线程完成
    #             waiter_thread = Thread(target=self.wait_for_completion, args=(threads,))
    #             waiter_thread.setDaemon(True)
    #             waiter_thread.start()
    #             # res = self.__depth2laser_node_5_calibration_server_client(req)
    #         else:
    #             msg = "请求数据错误！camera_id = {}".format(camera_id)
    #             raise Exception(msg)
            
    #         # 发送话题，通知校准线程开始
            
    #         self.__calibrate_enable_pub.publish(True)
            
    #         # 获取结果            
    #         result = self.wait_for_data(60)
            
            
    #         if result == None:
    #             raise Exception("wait_for_data timeout.")
            

            

    #         if camera_name in result:
    #             if  result[camera_name]["result"] == False:
    #                 raise Exception("calibration failed.")
               
    #             ret_data = {}
    #             ret_data["pitch_deg"] = result["pitch_deg"]
    #             ret_data["roll_deg"] = result["roll_deg"]
    #             ret_data["yaw_deg"] = result["yaw_deg"]


    #         self.__co_calibration_result_flag = False
    #         self.__co_calibration_result = {}
    #         self.__robot_status['enable'] = 1.0
    #         self.__robot_status['angle'] = 0.0
    #         self.__robot_status['distance'] = 0.0
            
            
            
    #         # ret_data = {}
    #         # if res.success == True:
    #         #     # print(res.message)
    #         #     message = json.loads(res.message)
                
    #         #     ret_data["pitch_deg"] = message["pitch"]
    #         #     ret_data["roll_deg"] = message["roll"]
    #         #     ret_data["yaw_deg"] = message["yaw"]


        
    #         ret_dat["data"] = ret_data
    #         log_msg = "[Robot_Parser][DepthInstall]深度相机标定camera_id = {} .".format(camera_id)
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
            
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser][DepthInstall]error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #         # rospy.logerr(err_msg)
    #     finally:
    #         return ret_dat


    # def PositionCalibration(self):
    #     '''
    #     一键联合校准下的开始位置校准
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     threads = []
    #     try:
    #         depth_camera_nums = rospy.get_param("/depth_camera_nums",2)
    #         # 创建并启动所有线程
    #         for i in range(1, depth_camera_nums + 1):
    #             thread = Thread(target=self.calibration_thread, args=(i,))
    #             thread.setDaemon(True)
    #             threads.append(thread)
    #             thread.start()
            
    #         # 启动一个线程来等待所有校准线程完成
    #         waiter_thread = Thread(target=self.wait_for_completion, args=(threads,))
    #         waiter_thread.setDaemon(True)
    #         waiter_thread.start()
                
    #     except Exception as error:
            
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser][PositionCalibration] error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #     return ret_dat
    
    # def calibration_thread(self, camera_id):
    #     try:
    #         calibration_method = getattr(self, 'PositionCameraCalibrationThread{}'.format(camera_id))
    #         result =   calibration_method()
    #         self.__co_calibration_result_callback_queue.put((camera_id, result))
    #     except Exception as e:
    #         self.__co_calibration_result_callback_queue.put((camera_id, 'error: {}'.format(str(e))))

    # def wait_for_completion(self, threads):
    #     for thread in threads:
    #         thread.join()
    #     self.process_callbacks()

    # def process_callbacks(self):
    #     while not self.__co_calibration_result_callback_queue.empty():
    #         camera_id, result = self.__co_calibration_result_callback_queue.get()
    #         if isinstance(result, dict):
    #             self.__co_calibration_result["camera_0{}".format(camera_id)] = result
    #             print("WWWWWWWWWWWWW",self.__co_calibration_result)
    #             rospy.loginfo("Camera {} calibration success: {}".format(camera_id, result))
    #         else:
    #             rospy.logerr("Camera {} calibration failed: {}".format(camera_id, result))
    #     self.__co_calibration_result_flag = True
    
    
    # def wait_for_data(self, timeout=None):
    #     rospy.loginfo("Waiting for data...")
    #     start_time = rospy.get_time()
    #     while self.__co_calibration_result_flag == False:
    #         if timeout is not None and rospy.get_time() - start_time > timeout:
    #             rospy.logwarn("Timeout waiting for data")
    #             return None
    #         rospy.sleep(0.1)
    #     return self.__co_calibration_result

    


    # def PositionCameraCalibrationThread1(self):
    #     try:
    #         print("++++++++++++++++++++++++++++++++++++++++++++PositionCameraCalibrationThread++++++++++++++++++++++++++++++++++++++++++++")
    #         req = SetBoolRequest()
    #         req.data = True
    #         res = SetBoolResponse()
                      
    #         print("============1=============")
    #         res = self.__depth2laser_node_1_calibration_server_client(req)
            
    #         return {
    #             'result': res.success,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }
                         
    #         log_msg = "[Robot_Parser][PositionCameraCalibrationThread]深度相机标定联合标定."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
    #         print(error)
        
    #         err_msg = "[Robot_Parser][PositionCameraCalibrationThread]error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
    #         return {
    #             'result': False,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }

    # def PositionCameraCalibrationThread2(self):
    #     try:
    #         print("++++++++++++++++++++++++++++++++++++++++++++PositionCameraCalibrationThread++++++++++++++++++++++++++++++++++++++++++++")
    #         req = SetBoolRequest()
    #         req.data = True
    #         res = SetBoolResponse()
            
    #         print("============2=============")
    #         res = self.__depth2laser_node_2_calibration_server_client(req)
            
    #         return {
    #             'result': res.success,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }
              
    #         log_msg = "[Robot_Parser][PositionCameraCalibrationThread]深度相机标定联合标定."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
    #         print(error)
        
    #         err_msg = "[Robot_Parser][PositionCameraCalibrationThread]error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
    #         return {
    #             'result': False,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }

    # def PositionCameraCalibrationThread3(self):
    #     try:
    #         print("++++++++++++++++++++++++++++++++++++++++++++PositionCameraCalibrationThread++++++++++++++++++++++++++++++++++++++++++++")
    #         req = SetBoolRequest()
    #         req.data = True
    #         res = SetBoolResponse()
            
    #         print("============3=============")
    #         res = self.__depth2laser_node_3_calibration_server_client(req)
            
    #         return {
    #             'result': res.success,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }
              
    #         log_msg = "[Robot_Parser][PositionCameraCalibrationThread]深度相机标定联合标定."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
    #         print(error)
        
    #         err_msg = "[Robot_Parser][PositionCameraCalibrationThread]error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
    #         return {
    #             'result': False,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }
            
    # def PositionCameraCalibrationThread4(self):
    #     try:
    #         print("++++++++++++++++++++++++++++++++++++++++++++PositionCameraCalibrationThread++++++++++++++++++++++++++++++++++++++++++++")
    #         req = SetBoolRequest()
    #         req.data = True
    #         res = SetBoolResponse()
            
    #         print("============4=============")
    #         res = self.__depth2laser_node_4_calibration_server_client(req)
            
    #         return {
    #             'result': res.success,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }
              
    #         log_msg = "[Robot_Parser][PositionCameraCalibrationThread]深度相机标定联合标定."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
    #         print(error)
        
    #         err_msg = "[Robot_Parser][PositionCameraCalibrationThread]error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
    #         return {
    #             'result': False,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }

    # def PositionCameraCalibrationThread5(self):
    #     try:
    #         print("++++++++++++++++++++++++++++++++++++++++++++PositionCameraCalibrationThread++++++++++++++++++++++++++++++++++++++++++++")
    #         req = SetBoolRequest()
    #         req.data = True
    #         res = SetBoolResponse()
            
    #         print("============5=============")
    #         res = self.__depth2laser_node_5_calibration_server_client(req)
            
    #         return {
    #             'result': res.success,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }
              
    #         log_msg = "[Robot_Parser][PositionCameraCalibrationThread]深度相机标定联合标定."
    #         log.LogInfo(self.GetNodeName(),log_msg)
    #     except Exception as error:
    #         print(error)
        
    #         err_msg = "[Robot_Parser][PositionCameraCalibrationThread]error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
    #         return {
    #             'result': False,
    #             'pitch_deg': 0.0,
    #             'roll_deg': 0.0,
    #             'yaw_deg': 0.0
    #         }
    
    # def PositionCalibrationCheckData(self):
    #     '''
    #     一键联合校准下的,开始位置校准后，获取相关信息
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     try:
    #         # 话题接受数据
    #         print("self.__robot_status>>>>>>>>>>>>",self.__robot_status)
    #         data = {
    #             'angle' : self.__robot_status['angle'],
    #             'distance':self.__robot_status['distance'],
    #             'enable':1 if self.__robot_status['enable'] == 0.0 else 0
    #         }
    #         ret_dat["data"] = data
    #         print(ret_dat)
            
    #     except Exception as error:
            
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser][PositionCalibrationCheckData] error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #     return ret_dat
    

    # def CoCalibration(self):
    #     '''
    #     一键联合校准下的开始相机校准
    #     '''
    #     ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
    #     try:
    #         depth_camera_nums = rospy.get_param("/depth_camera_nums",2)
    #         # 发送话题通知校准节点校准
    #         self.__calibrate_enable_pub.publish(True)
            
    #         # 获取结果            
    #         result = self.wait_for_data(60)
            
    #         print("result>>>>>>>>>>>>>>>>>>>>:",result) 
            
    #         if result == None:
    #             raise Exception("wait_for_data timeout.")
            
    #         camera_id_fail = []
            
    #         for i in range(1,depth_camera_nums + 1):
    #             camera_name = "camera_0{}".format(i)
    #             if camera_name in result:
    #                 if  result[camera_name]["result"] == False:
    #                     camera_id_fail.append(i)
    #             else:
    #                 camera_id_fail.append(i)
    #         # camera_id_fail.append(1)
            
    #         ret_dat["data"] ={
    #             "camera_id_fail": camera_id_fail
    #             } 

    #         self.__co_calibration_result_flag = False
    #         self.__co_calibration_result = {}
    #         self.__robot_status['enable'] = 1.0
    #         self.__robot_status['angle'] = 0.0
    #         self.__robot_status['distance'] = 0.0
            

            
    #     except Exception as error:
    #         self.__co_calibration_result_flag = False
    #         self.__co_calibration_result = {}
    #         self.__robot_status['enable'] = 1.0
    #         self.__robot_status['angle'] = 0.0
    #         self.__robot_status['distance'] = 0.0
    #         ret_dat["code"] = -1
    #         ret_dat["msg"] = str(error)

    #         err_msg = "[Robot_Parser][CoCalibration] error: {}".format(error)
    #         log.LogError(self.GetNodeName(),err_msg)
            
    #     return ret_dat


    # def CameraCalibrationStatus_Callback(self,msg):
    #     '''
    #     深度相机标定数据
    #     '''
    #     # self.__camera_calibration_status_info["serial"] = msg.serial # 对应相机序列号
    #     self.__camera_calibration_status_info["sensor_id"] = msg.sensor_id # 对应相机序列号

    #     self.__camera_calibration_status_info["roll_deg"] = msg.roll_deg # 绕x轴旋转角度,单位度数
    #     self.__camera_calibration_status_info["pitch_deg"] = msg.pitch_deg # 绕y轴旋转角度,单位度数
    #     self.__camera_calibration_status_info["yaw_deg"] = msg.yaw_deg # 绕z轴旋转角度,单位度数
    #     self.__camera_calibration_status_info["install_x"] = msg.install_x # 相机安装位姿x
    #     self.__camera_calibration_status_info["install_y"] = msg.install_y # 相机安装位姿y
    #     self.__camera_calibration_status_info["install_z"] = msg.install_z # 相机安装位姿z

    #     # self.__camera_calibration_status_info["front_distance"] = msg.front_distance # 机器人前向距离，正前方0°，用于雷达校准(sensor_id=LIDAR_2D

    # def RobotStatus_Callback(self,msg = Float32MultiArray):
    #     '''
    #     机器人摆放位姿 20240513-dw.liu
    #     {angle, distance, 0.0};//依次为：角度，距离，使能（0.0可校准,其他值为不可校准）
    #     '''
    #     self.__robot_status = {
    #         'angle':msg.data[0],
    #         'distance':msg.data[1],
    #         'enable':msg.data[2]
    #     }